14 files changed, 116 insertions, 58 deletions

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