MFC r304285:

Implement userspace gettimeofday(2) with HPET timecounter.

24 files changed, 246 insertions, 171 deletions

diff --git a/lib/libc/Makefile b/lib/libc/Makefile
index e511c9b..2e17ec4 100644
--- a/lib/libc/Makefile
+++ b/lib/libc/Makefile
@@ -110,6 +110,9 @@ NOASM=
      ${LIBC_ARCH} == "mips"
 .include "${LIBC_SRCTOP}/softfloat/Makefile.inc"
 .endif
+.if ${LIBC_ARCH} == "i386" || ${LIBC_ARCH} == "amd64"
+.include "${LIBC_SRCTOP}/x86/sys/Makefile.inc"
+.endif
 .if ${MK_NIS} != "no"
 CFLAGS+= -DYP
 .include "${LIBC_SRCTOP}/yp/Makefile.inc"
diff --git a/lib/libc/aarch64/sys/__vdso_gettc.c b/lib/libc/aarch64/sys/__vdso_gettc.c
index f9015e2..85e1b2e 100644
--- a/lib/libc/aarch64/sys/__vdso_gettc.c
+++ b/lib/libc/aarch64/sys/__vdso_gettc.c
@@ -34,6 +34,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/time.h>
 #include <sys/vdso.h>
 #include <machine/cpufunc.h>
+#include <errno.h>
 #include "libc_private.h"
 
 static inline uint64_t
@@ -55,14 +56,15 @@ cp15_cntpct_get(void)
 }
 
 #pragma weak __vdso_gettc
-u_int
-__vdso_gettc(const struct vdso_timehands *th)
+int
+__vdso_gettc(const struct vdso_timehands *th, u_int *tc)
 {
-	uint64_t val;
 
+	if (th->th_algo != VDSO_TH_ALGO_ARM_GENTIM)
+		return (ENOSYS);
 	__asm __volatile("isb" : : : "memory");
-	val = th->th_physical == 0 ? cp15_cntvct_get() : cp15_cntpct_get();
-	return (val);
+	*tc = th->th_physical == 0 ? cp15_cntvct_get() : cp15_cntpct_get();
+	return (0);
 }
 
 #pragma weak __vdso_gettimekeep
diff --git a/lib/libc/amd64/sys/Makefile.inc b/lib/libc/amd64/sys/Makefile.inc
index d5d416f..078a017 100644
--- a/lib/libc/amd64/sys/Makefile.inc
+++ b/lib/libc/amd64/sys/Makefile.inc
@@ -2,7 +2,7 @@
 # $FreeBSD$
 
 SRCS+=	amd64_get_fsbase.c amd64_get_gsbase.c amd64_set_fsbase.c \
-	amd64_set_gsbase.c __vdso_gettc.c
+	amd64_set_gsbase.c
 
 MDASM=	vfork.S brk.S cerror.S exect.S getcontext.S \
 	sbrk.S setlogin.S sigreturn.S
diff --git a/lib/libc/amd64/sys/__vdso_gettc.c b/lib/libc/amd64/sys/__vdso_gettc.c
deleted file mode 100644
index 1899b21..0000000
--- a/lib/libc/amd64/sys/__vdso_gettc.c
+++ /dev/null
@@ -1,70 +0,0 @@
-/*-
- * Copyright (c) 2012 Konstantin Belousov <kib@FreeBSD.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- */
-
-#include <sys/cdefs.h>
-__FBSDID("$FreeBSD$");
-
-#include <sys/types.h>
-#include <sys/elf.h>
-#include <sys/time.h>
-#include <sys/vdso.h>
-#include <machine/cpufunc.h>
-#include "libc_private.h"
-
-static u_int
-__vdso_gettc_low(const struct vdso_timehands *th)
-{
-	u_int rv;
-
-	__asm __volatile("lfence; rdtsc; shrd %%cl, %%edx, %0"
-	    : "=a" (rv) : "c" (th->th_x86_shift) : "edx");
-	return (rv);
-}
-
-static u_int
-__vdso_rdtsc32(void)
-{
-	u_int rv;
-
-	__asm __volatile("lfence;rdtsc" : "=a" (rv) : : "edx");
-	return (rv);
-}
-
-#pragma weak __vdso_gettc
-u_int
-__vdso_gettc(const struct vdso_timehands *th)
-{
-
-	return (th->th_x86_shift > 0 ? __vdso_gettc_low(th) :
-	    __vdso_rdtsc32());
-}
-
-#pragma weak __vdso_gettimekeep
-int
-__vdso_gettimekeep(struct vdso_timekeep **tk)
-{
-
-	return (_elf_aux_info(AT_TIMEKEEP, tk, sizeof(*tk)));
-}
diff --git a/lib/libc/arm/sys/__vdso_gettc.c b/lib/libc/arm/sys/__vdso_gettc.c
index 1f43e72..9f8631d 100644
--- a/lib/libc/arm/sys/__vdso_gettc.c
+++ b/lib/libc/arm/sys/__vdso_gettc.c
@@ -35,6 +35,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/vdso.h>
 #include <machine/cpufunc.h>
 #include <machine/acle-compat.h>
+#include <errno.h>
 #include "libc_private.h"
 
 #if __ARM_ARCH >= 6
@@ -58,11 +59,12 @@ cp15_cntpct_get(void)
 #endif
 
 #pragma weak __vdso_gettc
-u_int
-__vdso_gettc(const struct vdso_timehands *th)
+int
+__vdso_gettc(const struct vdso_timehands *th, u_int *tc)
 {
-	uint64_t val;
 
+	if (th->th_algo != VDSO_TH_ALGO_ARM_GENTIM)
+		return (ENOSYS);
 #if __ARM_ARCH >= 6
 	/*
 	 * Userspace gettimeofday() is only enabled on ARMv7 CPUs, but
@@ -70,11 +72,12 @@ __vdso_gettc(const struct vdso_timehands *th)
 	 * armv7-a directive does not work.
 	 */
 	__asm __volatile(".word\t0xf57ff06f" : : : "memory"); /* isb */
-	val = th->th_physical == 0 ? cp15_cntvct_get() : cp15_cntpct_get();
+	*tc = th->th_physical == 0 ? cp15_cntvct_get() : cp15_cntpct_get();
+	return (0);
 #else
-	val = 0;
+	*tc = 0;
+	return (ENOSYS);
 #endif
-	return (val);
 }
 
 #pragma weak __vdso_gettimekeep
diff --git a/lib/libc/i386/sys/Makefile.inc b/lib/libc/i386/sys/Makefile.inc
index 9bc2a56..3f96bd9 100644
--- a/lib/libc/i386/sys/Makefile.inc
+++ b/lib/libc/i386/sys/Makefile.inc
@@ -5,8 +5,7 @@
 SRCS+=	i386_clr_watch.c i386_set_watch.c i386_vm86.c
 .endif
 SRCS+=	i386_get_fsbase.c i386_get_gsbase.c i386_get_ioperm.c i386_get_ldt.c \
-	i386_set_fsbase.c i386_set_gsbase.c i386_set_ioperm.c i386_set_ldt.c \
-	__vdso_gettc.c
+	i386_set_fsbase.c i386_set_gsbase.c i386_set_ioperm.c i386_set_ldt.c
 
 MDASM=	Ovfork.S brk.S cerror.S exect.S getcontext.S \
 	sbrk.S setlogin.S sigreturn.S syscall.S
diff --git a/lib/libc/sys/__vdso_gettimeofday.c b/lib/libc/sys/__vdso_gettimeofday.c
index b3527fa..0368b97 100644
--- a/lib/libc/sys/__vdso_gettimeofday.c
+++ b/lib/libc/sys/__vdso_gettimeofday.c
@@ -34,12 +34,16 @@ __FBSDID("$FreeBSD$");
 #include <machine/atomic.h>
 #include "libc_private.h"
 
-static u_int
-tc_delta(const struct vdso_timehands *th)
+static int
+tc_delta(const struct vdso_timehands *th, u_int *delta)
 {
+	int error;
+	u_int tc;
 
-	return ((__vdso_gettc(th) - th->th_offset_count) &
-	    th->th_counter_mask);
+	error = __vdso_gettc(th, &tc);
+	if (error == 0)
+		*delta = (tc - th->th_offset_count) & th->th_counter_mask;
+	return (error);
 }
 
 /*
@@ -56,6 +60,8 @@ binuptime(struct bintime *bt, struct vdso_timekeep *tk, int abs)
 {
 	struct vdso_timehands *th;
 	uint32_t curr, gen;
+	u_int delta;
+	int error;
 
 	do {
 		if (!tk->tk_enabled)
@@ -63,11 +69,14 @@ binuptime(struct bintime *bt, struct vdso_timekeep *tk, int abs)
 
 		curr = atomic_load_acq_32(&tk->tk_current);
 		th = &tk->tk_th[curr];
-		if (th->th_algo != VDSO_TH_ALGO_1)
-			return (ENOSYS);
 		gen = atomic_load_acq_32(&th->th_gen);
 		*bt = th->th_offset;
-		bintime_addx(bt, th->th_scale * tc_delta(th));
+		error = tc_delta(th, &delta);
+		if (error == EAGAIN)
+			continue;
+		if (error != 0)
+			return (error);
+		bintime_addx(bt, th->th_scale * delta);
 		if (abs)
 			bintime_add(bt, &th->th_boottime);
 
diff --git a/lib/libc/sys/trivial-vdso_tc.c b/lib/libc/sys/trivial-vdso_tc.c
index b99bbc4..ce0c144 100644
--- a/lib/libc/sys/trivial-vdso_tc.c
+++ b/lib/libc/sys/trivial-vdso_tc.c
@@ -32,11 +32,11 @@ __FBSDID("$FreeBSD$");
 #include <errno.h>
 
 #pragma weak __vdso_gettc
-u_int
-__vdso_gettc(const struct vdso_timehands *th)
+int
+__vdso_gettc(const struct vdso_timehands *th, u_int *tc)
 {
 
-	return (0);
+	return (ENOSYS);
 }
 
 #pragma weak __vdso_gettimekeep
diff --git a/lib/libc/x86/sys/Makefile.inc b/lib/libc/x86/sys/Makefile.inc
new file mode 100644
index 0000000..c00a217
--- /dev/null
+++ b/lib/libc/x86/sys/Makefile.inc
@@ -0,0 +1,6 @@
+# $FreeBSD$
+
+.PATH:	${LIBC_SRCTOP}/x86/sys
+
+SRCS+= \
+	__vdso_gettc.c
diff --git a/lib/libc/i386/sys/__vdso_gettc.c b/lib/libc/x86/sys/__vdso_gettc.c
index 1454f16..743b3ad 100644
--- a/lib/libc/i386/sys/__vdso_gettc.c
+++ b/lib/libc/x86/sys/__vdso_gettc.c
@@ -1,5 +1,10 @@
 /*-
  * Copyright (c) 2012 Konstantin Belousov <kib@FreeBSD.org>
+ * Copyright (c) 2016 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * Portions of this software were developed by Konstantin Belousov
+ * under sponsorship from the FreeBSD Foundation.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -26,19 +31,27 @@
 #include <sys/cdefs.h>
 __FBSDID("$FreeBSD$");
 
-#include <sys/types.h>
+#include <sys/param.h>
+#include "namespace.h"
 #include <sys/elf.h>
+#include <sys/fcntl.h>
+#include <sys/mman.h>
 #include <sys/time.h>
 #include <sys/vdso.h>
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include "un-namespace.h"
 #include <machine/cpufunc.h>
 #include <machine/specialreg.h>
+#include <dev/acpica/acpi_hpet.h>
 #include "libc_private.h"
 
-static int lfence_works = -1;
-
-static int
-get_lfence_usage(void)
+static void
+lfence_mb(void)
 {
+#if defined(__i386__)
+	static int lfence_works = -1;
 	u_int cpuid_supported, p[4];
 
 	if (lfence_works == -1) {
@@ -57,7 +70,7 @@ get_lfence_usage(void)
 		    "	jmp	2f\n"
 		    "1:	movl	$0,%0\n"
 		    "2:\n"
-		    : "=r" (cpuid_supported) : : "eax", "ecx");
+		    : "=r" (cpuid_supported) : : "eax", "ecx", "cc");
 		if (cpuid_supported) {
 			__asm __volatile(
 			    "	pushl	%%ebx\n"
@@ -70,16 +83,21 @@ get_lfence_usage(void)
 		} else
 			lfence_works = 0;
 	}
-	return (lfence_works);
+	if (lfence_works == 1)
+		lfence();
+#elif defined(__amd64__)
+	lfence();
+#else
+#error "arch"
+#endif
 }
 
 static u_int
-__vdso_gettc_low(const struct vdso_timehands *th)
+__vdso_gettc_rdtsc_low(const struct vdso_timehands *th)
 {
 	u_int rv;
 
-	if (get_lfence_usage() == 1)
-		lfence();
+	lfence_mb();
 	__asm __volatile("rdtsc; shrd %%cl, %%edx, %0"
 	    : "=a" (rv) : "c" (th->th_x86_shift) : "edx");
 	return (rv);
@@ -88,21 +106,68 @@ __vdso_gettc_low(const struct vdso_timehands *th)
 static u_int
 __vdso_rdtsc32(void)
 {
-	u_int rv;
 
-	if (get_lfence_usage() == 1)
-		lfence();
-	rv = rdtsc32();
-	return (rv);
+	lfence_mb();
+	return (rdtsc32());
+}
+
+static char *hpet_dev_map = NULL;
+static uint32_t hpet_idx = 0xffffffff;
+
+static void
+__vdso_init_hpet(uint32_t u)
+{
+	static const char devprefix[] = "/dev/hpet";
+	char devname[64], *c, *c1, t;
+	int fd;
+
+	c1 = c = stpcpy(devname, devprefix);
+	u = hpet_idx;
+	do {
+		*c++ = u % 10 + '0';
+		u /= 10;
+	} while (u != 0);
+	*c = '\0';
+	for (c--; c1 != c; c1++, c--) {
+		t = *c1;
+		*c1 = *c;
+		*c = t;
+	}
+	fd = _open(devname, O_RDONLY);
+	if (fd == -1) {
+		hpet_dev_map = MAP_FAILED;
+		return;
+	}
+	if (hpet_dev_map != NULL && hpet_dev_map != MAP_FAILED)
+		munmap(hpet_dev_map, PAGE_SIZE);
+	hpet_dev_map = mmap(NULL, PAGE_SIZE, PROT_READ, MAP_SHARED, fd, 0);
+	_close(fd);
 }
 
 #pragma weak __vdso_gettc
-u_int
-__vdso_gettc(const struct vdso_timehands *th)
+int
+__vdso_gettc(const struct vdso_timehands *th, u_int *tc)
 {
+	uint32_t tmp;
 
-	return (th->th_x86_shift > 0 ? __vdso_gettc_low(th) :
-	    __vdso_rdtsc32());
+	switch (th->th_algo) {
+	case VDSO_TH_ALGO_X86_TSC:
+		*tc = th->th_x86_shift > 0 ? __vdso_gettc_rdtsc_low(th) :
+		    __vdso_rdtsc32();
+		return (0);
+	case VDSO_TH_ALGO_X86_HPET:
+		tmp = th->th_x86_hpet_idx;
+		if (hpet_dev_map == NULL || tmp != hpet_idx) {
+			hpet_idx = tmp;
+			__vdso_init_hpet(hpet_idx);
+		}
+		if (hpet_dev_map == MAP_FAILED)
+			return (ENOSYS);
+		*tc = *(volatile uint32_t *)(hpet_dev_map + HPET_MAIN_COUNTER);
+		return (0);
+	default:
+		return (ENOSYS);
+	}
 }
 
 #pragma weak __vdso_gettimekeep
diff --git a/sys/arm/arm/generic_timer.c b/sys/arm/arm/generic_timer.c
index 7cf862b..47fe459 100644
--- a/sys/arm/arm/generic_timer.c
+++ b/sys/arm/arm/generic_timer.c
@@ -105,6 +105,10 @@ static struct resource_spec timer_spec[] = {
 	{ -1, 0 }
 };
 
+static uint32_t arm_tmr_fill_vdso_timehands(struct vdso_timehands *vdso_th,
+    struct timecounter *tc);
+static void arm_tmr_do_delay(int usec, void *);
+
 static timecounter_get_t arm_tmr_get_timecount;
 
 static struct timecounter arm_tmr_timecount = {
@@ -114,6 +118,7 @@ static struct timecounter arm_tmr_timecount = {
 	.tc_counter_mask   = ~0u,
 	.tc_frequency      = 0,
 	.tc_quality        = 1000,
+	.tc_fill_vdso_timehands = arm_tmr_fill_vdso_timehands,
 };
 
 #ifdef __arm__
@@ -128,10 +133,6 @@ static struct timecounter arm_tmr_timecount = {
 #define	set_el1(x, val)	WRITE_SPECIALREG(x ##_el1, val)
 #endif
 
-static uint32_t arm_tmr_fill_vdso_timehands(struct vdso_timehands *vdso_th,
-    struct timecounter *tc);
-static void arm_tmr_do_delay(int usec, void *);
-
 static int
 get_freq(void)
 {
@@ -412,8 +413,6 @@ arm_tmr_attach(device_t dev)
 		}
 	}
 
-	arm_cpu_fill_vdso_timehands = arm_tmr_fill_vdso_timehands;
-
 	arm_tmr_timecount.tc_frequency = sc->clkfreq;
 	tc_init(&arm_tmr_timecount);
 
@@ -535,7 +534,8 @@ arm_tmr_fill_vdso_timehands(struct vdso_timehands *vdso_th,
     struct timecounter *tc)
 {
 
+	vdso_th->th_algo = VDSO_TH_ALGO_ARM_GENTIM;
 	vdso_th->th_physical = arm_tmr_sc->physical;
 	bzero(vdso_th->th_res, sizeof(vdso_th->th_res));
-	return (tc == &arm_tmr_timecount);
+	return (1);
 }
diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 960fa6a..1a584fe 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -1996,14 +1996,3 @@ initarm(struct arm_boot_params *abp)
 
 #endif /* __ARM_ARCH < 6 */
 #endif /* FDT */
-
-uint32_t (*arm_cpu_fill_vdso_timehands)(struct vdso_timehands *,
-    struct timecounter *);
-
-uint32_t
-cpu_fill_vdso_timehands(struct vdso_timehands *vdso_th, struct timecounter *tc)
-{
-
-	return (arm_cpu_fill_vdso_timehands != NULL ?
-	    arm_cpu_fill_vdso_timehands(vdso_th, tc) : 0);
-}
diff --git a/sys/arm/include/md_var.h b/sys/arm/include/md_var.h
index 981c0b3..642124d 100644
--- a/sys/arm/include/md_var.h
+++ b/sys/arm/include/md_var.h
@@ -45,11 +45,6 @@ extern int (*_arm_bzero)(void *, int, int);
 extern int _min_memcpy_size;
 extern int _min_bzero_size;
 
-struct vdso_timehands;
-struct timecounter;
-extern uint32_t (*arm_cpu_fill_vdso_timehands)(struct vdso_timehands *,
-    struct timecounter *);
-
 #define DST_IS_USER	0x1
 #define SRC_IS_USER	0x2
 #define IS_PHYSICAL	0x4
diff --git a/sys/arm/include/vdso.h b/sys/arm/include/vdso.h
index 6a0ec7b..7bb4000 100644
--- a/sys/arm/include/vdso.h
+++ b/sys/arm/include/vdso.h
@@ -32,4 +32,6 @@
 	uint32_t	th_physical;		\
 	uint32_t	th_res[7];
 
+#define	VDSO_TH_ALGO_ARM_GENTIM	VDSO_TH_ALGO_1
+
 #endif
diff --git a/sys/arm64/arm64/machdep.c b/sys/arm64/arm64/machdep.c
index 9d4cfb1..6f5d1a8 100644
--- a/sys/arm64/arm64/machdep.c
+++ b/sys/arm64/arm64/machdep.c
@@ -927,17 +927,6 @@ initarm(struct arm64_bootparams *abp)
 	early_boot = 0;
 }
 
-uint32_t (*arm_cpu_fill_vdso_timehands)(struct vdso_timehands *,
-    struct timecounter *);
-
-uint32_t
-cpu_fill_vdso_timehands(struct vdso_timehands *vdso_th, struct timecounter *tc)
-{
-
-	return (arm_cpu_fill_vdso_timehands != NULL ?
-	    arm_cpu_fill_vdso_timehands(vdso_th, tc) : 0);
-}
-
 #ifdef DDB
 #include <ddb/ddb.h>
 
diff --git a/sys/arm64/include/md_var.h b/sys/arm64/include/md_var.h
index 07f816a..ef64920 100644
--- a/sys/arm64/include/md_var.h
+++ b/sys/arm64/include/md_var.h
@@ -47,9 +47,4 @@ void dump_add_page(vm_paddr_t);
 void dump_drop_page(vm_paddr_t);
 int minidumpsys(struct dumperinfo *);
 
-struct vdso_timehands;
-struct timecounter;
-extern uint32_t (*arm_cpu_fill_vdso_timehands)(struct vdso_timehands *,
-    struct timecounter *);
-
 #endif /* !_MACHINE_MD_VAR_H_ */
diff --git a/sys/arm64/include/vdso.h b/sys/arm64/include/vdso.h
index 285e986..872a8c0 100644
--- a/sys/arm64/include/vdso.h
+++ b/sys/arm64/include/vdso.h
@@ -32,4 +32,6 @@
 	uint32_t	th_physical;		\
 	uint32_t	th_res[7];
 
+#define	VDSO_TH_ALGO_ARM_GENTIM	VDSO_TH_ALGO_1
+
 #endif /* !_MACHINE_VDSO_H_ */
diff --git a/sys/dev/acpica/acpi_hpet.c b/sys/dev/acpica/acpi_hpet.c
index 2d4bac8..053b956 100644
--- a/sys/dev/acpica/acpi_hpet.c
+++ b/sys/dev/acpica/acpi_hpet.c
@@ -29,6 +29,8 @@
 __FBSDID("$FreeBSD$");
 
 #include "opt_acpi.h"
+#include "opt_compat.h"
+
 #if defined(__amd64__)
 #define	DEV_APIC
 #else
@@ -47,6 +49,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/sysctl.h>
 #include <sys/timeet.h>
 #include <sys/timetc.h>
+#include <sys/vdso.h>
 
 #include <contrib/dev/acpica/include/acpi.h>
 #include <contrib/dev/acpica/include/accommon.h>
@@ -141,6 +144,35 @@ hpet_get_timecount(struct timecounter *tc)
 	return (bus_read_4(sc->mem_res, HPET_MAIN_COUNTER));
 }
 
+uint32_t
+hpet_vdso_timehands(struct vdso_timehands *vdso_th, struct timecounter *tc)
+{
+	struct hpet_softc *sc;
+
+	sc = tc->tc_priv;
+	vdso_th->th_algo = VDSO_TH_ALGO_X86_HPET;
+	vdso_th->th_x86_shift = 0;
+	vdso_th->th_x86_hpet_idx = device_get_unit(sc->dev);
+	bzero(vdso_th->th_res, sizeof(vdso_th->th_res));
+	return (sc->mmap_allow != 0);
+}
+
+#ifdef COMPAT_FREEBSD32
+uint32_t
+hpet_vdso_timehands32(struct vdso_timehands32 *vdso_th32,
+    struct timecounter *tc)
+{
+	struct hpet_softc *sc;
+
+	sc = tc->tc_priv;
+	vdso_th32->th_algo = VDSO_TH_ALGO_X86_HPET;
+	vdso_th32->th_x86_shift = 0;
+	vdso_th32->th_x86_hpet_idx = device_get_unit(sc->dev);
+	bzero(vdso_th32->th_res, sizeof(vdso_th32->th_res));
+	return (sc->mmap_allow != 0);
+}
+#endif
+
 static void
 hpet_enable(struct hpet_softc *sc)
 {
@@ -537,6 +569,10 @@ hpet_attach(device_t dev)
 		sc->tc.tc_quality = 950,
 		sc->tc.tc_frequency = sc->freq;
 		sc->tc.tc_priv = sc;
+		sc->tc.tc_fill_vdso_timehands = hpet_vdso_timehands;
+#ifdef COMPAT_FREEBSD32
+		sc->tc.tc_fill_vdso_timehands32 = hpet_vdso_timehands32;
+#endif
 		tc_init(&sc->tc);
 	}
 	/* If not disabled - setup and announce event timers. */
diff --git a/sys/dev/acpica/acpi_hpet.h b/sys/dev/acpica/acpi_hpet.h
index fd495d5..c3941c3 100644
--- a/sys/dev/acpica/acpi_hpet.h
+++ b/sys/dev/acpica/acpi_hpet.h
@@ -64,4 +64,15 @@
 
 #define	HPET_MIN_CYCLES		128	/* Period considered reliable. */
 
+#ifdef _KERNEL
+struct timecounter;
+struct vdso_timehands;
+struct vdso_timehands32;
+
+uint32_t hpet_vdso_timehands(struct vdso_timehands *vdso_th,
+    struct timecounter *tc);
+uint32_t hpet_vdso_timehands32(struct vdso_timehands32 *vdso_th32,
+    struct timecounter *tc);
+#endif
+
 #endif /* !__ACPI_HPET_H__ */
diff --git a/sys/kern/kern_tc.c b/sys/kern/kern_tc.c
index 4319123..9cc5f42 100644
--- a/sys/kern/kern_tc.c
+++ b/sys/kern/kern_tc.c
@@ -6,11 +6,14 @@
  * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
  * ----------------------------------------------------------------------------
  *
- * Copyright (c) 2011 The FreeBSD Foundation
+ * Copyright (c) 2011, 2015, 2016 The FreeBSD Foundation
  * All rights reserved.
  *
  * Portions of this software were developed by Julien Ridoux at the University
  * of Melbourne under sponsorship from the FreeBSD Foundation.
+ *
+ * Portions of this software were developed by Konstantin Belousov
+ * under sponsorship from the FreeBSD Foundation.
  */
 
 #include <sys/cdefs.h>
@@ -2134,13 +2137,16 @@ tc_fill_vdso_timehands(struct vdso_timehands *vdso_th)
 	uint32_t enabled;
 
 	th = timehands;
-	vdso_th->th_algo = VDSO_TH_ALGO_1;
 	vdso_th->th_scale = th->th_scale;
 	vdso_th->th_offset_count = th->th_offset_count;
 	vdso_th->th_counter_mask = th->th_counter->tc_counter_mask;
 	vdso_th->th_offset = th->th_offset;
 	vdso_th->th_boottime = th->th_boottime;
-	enabled = cpu_fill_vdso_timehands(vdso_th, th->th_counter);
+	if (th->th_counter->tc_fill_vdso_timehands != NULL) {
+		enabled = th->th_counter->tc_fill_vdso_timehands(vdso_th,
+		    th->th_counter);
+	} else
+		enabled = 0;
 	if (!vdso_th_enable)
 		enabled = 0;
 	return (enabled);
@@ -2154,7 +2160,6 @@ tc_fill_vdso_timehands32(struct vdso_timehands32 *vdso_th32)
 	uint32_t enabled;
 
 	th = timehands;
-	vdso_th32->th_algo = VDSO_TH_ALGO_1;
 	*(uint64_t *)&vdso_th32->th_scale[0] = th->th_scale;
 	vdso_th32->th_offset_count = th->th_offset_count;
 	vdso_th32->th_counter_mask = th->th_counter->tc_counter_mask;
@@ -2162,7 +2167,11 @@ tc_fill_vdso_timehands32(struct vdso_timehands32 *vdso_th32)
 	*(uint64_t *)&vdso_th32->th_offset.frac[0] = th->th_offset.frac;
 	vdso_th32->th_boottime.sec = th->th_boottime.sec;
 	*(uint64_t *)&vdso_th32->th_boottime.frac[0] = th->th_boottime.frac;
-	enabled = cpu_fill_vdso_timehands32(vdso_th32, th->th_counter);
+	if (th->th_counter->tc_fill_vdso_timehands32 != NULL) {
+		enabled = th->th_counter->tc_fill_vdso_timehands32(vdso_th32,
+		    th->th_counter);
+	} else
+		enabled = 0;
 	if (!vdso_th_enable)
 		enabled = 0;
 	return (enabled);
diff --git a/sys/sys/timetc.h b/sys/sys/timetc.h
index 8f00e22..f4cce45 100644
--- a/sys/sys/timetc.h
+++ b/sys/sys/timetc.h
@@ -28,8 +28,14 @@
  */
 
 struct timecounter;
+struct vdso_timehands;
+struct vdso_timehands32;
 typedef u_int timecounter_get_t(struct timecounter *);
 typedef void timecounter_pps_t(struct timecounter *);
+typedef uint32_t timecounter_fill_vdso_timehands_t(struct vdso_timehands *,
+    struct timecounter *);
+typedef uint32_t timecounter_fill_vdso_timehands32_t(struct vdso_timehands32 *,
+    struct timecounter *);
 
 struct timecounter {
 	timecounter_get_t	*tc_get_timecount;
@@ -68,6 +74,8 @@ struct timecounter {
 		/* Pointer to the timecounter's private parts. */
 	struct timecounter	*tc_next;
 		/* Pointer to the next timecounter. */
+	timecounter_fill_vdso_timehands_t *tc_fill_vdso_timehands;
+	timecounter_fill_vdso_timehands32_t *tc_fill_vdso_timehands32;
 };
 
 extern struct timecounter *timecounter;
diff --git a/sys/sys/vdso.h b/sys/sys/vdso.h
index 907f4db..db7eea7 100644
--- a/sys/sys/vdso.h
+++ b/sys/sys/vdso.h
@@ -53,6 +53,7 @@ struct vdso_timekeep {
 #define	VDSO_TK_VER_1		0x1
 #define	VDSO_TK_VER_CURR	VDSO_TK_VER_1
 #define	VDSO_TH_ALGO_1		0x1
+#define	VDSO_TH_ALGO_2		0x2
 
 #ifndef _KERNEL
 
@@ -62,7 +63,7 @@ struct timezone;
 
 int __vdso_clock_gettime(clockid_t clock_id, struct timespec *ts);
 int __vdso_gettimeofday(struct timeval *tv, struct timezone *tz);
-u_int __vdso_gettc(const struct vdso_timehands *vdso_th);
+int __vdso_gettc(const struct vdso_timehands *vdso_th, u_int *tc);
 int __vdso_gettimekeep(struct vdso_timekeep **tk);
 
 #endif
diff --git a/sys/x86/include/vdso.h b/sys/x86/include/vdso.h
index 1171420..0744665 100644
--- a/sys/x86/include/vdso.h
+++ b/sys/x86/include/vdso.h
@@ -1,7 +1,11 @@
 /*-
  * Copyright 2012 Konstantin Belousov <kib@FreeBSD.ORG>.
+ * Copyright 2016 The FreeBSD Foundation.
  * All rights reserved.
  *
+ * Portions of this software were developed by Konstantin Belousov
+ * under sponsorship from the FreeBSD Foundation.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
@@ -30,7 +34,11 @@
 
 #define	VDSO_TIMEHANDS_MD			\
 	uint32_t	th_x86_shift;		\
-	uint32_t	th_res[7];
+	uint32_t	th_x86_hpet_idx;	\
+	uint32_t	th_res[6];
+
+#define	VDSO_TH_ALGO_X86_TSC	VDSO_TH_ALGO_1
+#define	VDSO_TH_ALGO_X86_HPET	VDSO_TH_ALGO_2
 
 #ifdef _KERNEL
 #ifdef COMPAT_FREEBSD32
diff --git a/sys/x86/x86/tsc.c b/sys/x86/x86/tsc.c
index a0b0f42..3f36fbd 100644
--- a/sys/x86/x86/tsc.c
+++ b/sys/x86/x86/tsc.c
@@ -48,6 +48,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/md_var.h>
 #include <machine/specialreg.h>
 #include <x86/vmware.h>
+#include <dev/acpica/acpi_hpet.h>
 
 #include "cpufreq_if.h"
 
@@ -93,14 +94,22 @@ static unsigned tsc_get_timecount_low_lfence(struct timecounter *tc);
 static unsigned tsc_get_timecount_mfence(struct timecounter *tc);
 static unsigned tsc_get_timecount_low_mfence(struct timecounter *tc);
 static void tsc_levels_changed(void *arg, int unit);
+static uint32_t x86_tsc_vdso_timehands(struct vdso_timehands *vdso_th,
+    struct timecounter *tc);
+#ifdef COMPAT_FREEBSD32
+static uint32_t x86_tsc_vdso_timehands32(struct vdso_timehands32 *vdso_th32,
+    struct timecounter *tc);
+#endif
 
 static struct timecounter tsc_timecounter = {
-	tsc_get_timecount,	/* get_timecount */
-	0,			/* no poll_pps */
-	~0u,			/* counter_mask */
-	0,			/* frequency */
-	"TSC",			/* name */
-	800,			/* quality (adjusted in code) */
+	.tc_get_timecount =		tsc_get_timecount,
+	.tc_counter_mask =		~0u,
+	.tc_name =			"TSC",
+	.tc_quality =			800,	/* adjusted in code */
+	.tc_fill_vdso_timehands = 	x86_tsc_vdso_timehands,
+#ifdef COMPAT_FREEBSD32
+	.tc_fill_vdso_timehands32 = 	x86_tsc_vdso_timehands32,
+#endif
 };
 
 static void
@@ -724,23 +733,27 @@ tsc_get_timecount_low_mfence(struct timecounter *tc)
 	return (tsc_get_timecount_low(tc));
 }
 
-uint32_t
-cpu_fill_vdso_timehands(struct vdso_timehands *vdso_th, struct timecounter *tc)
+static uint32_t
+x86_tsc_vdso_timehands(struct vdso_timehands *vdso_th, struct timecounter *tc)
 {
 
+	vdso_th->th_algo = VDSO_TH_ALGO_X86_TSC;
 	vdso_th->th_x86_shift = (int)(intptr_t)tc->tc_priv;
+	vdso_th->th_x86_hpet_idx = 0xffffffff;
 	bzero(vdso_th->th_res, sizeof(vdso_th->th_res));
-	return (tc == &tsc_timecounter);
+	return (1);
 }
 
 #ifdef COMPAT_FREEBSD32
-uint32_t
-cpu_fill_vdso_timehands32(struct vdso_timehands32 *vdso_th32,
+static uint32_t
+x86_tsc_vdso_timehands32(struct vdso_timehands32 *vdso_th32,
     struct timecounter *tc)
 {
 
+	vdso_th32->th_algo = VDSO_TH_ALGO_X86_TSC;
 	vdso_th32->th_x86_shift = (int)(intptr_t)tc->tc_priv;
+	vdso_th32->th_x86_hpet_idx = 0xffffffff;
 	bzero(vdso_th32->th_res, sizeof(vdso_th32->th_res));
-	return (tc == &tsc_timecounter);
+	return (1);
 }
 #endif