From 26492a28950fbe89e280b6888f04509f48b4ee50 Mon Sep 17 00:00:00 2001
From: NAKAMURA Gou <go.nakamura.yw@hitachi-solutions.com>
Date: Wed, 11 Nov 2015 17:38:22 +0900
Subject: [PATCH] vsyscall_gettimeofday: make timeval from TSC

---
 arch/x86/kernel/vsyscall.c     | 76 ++++++++++++++++++++++++++++++++--
 kernel/config/attached-mic.lds |  1 +
 kernel/config/builtin-mic.lds  |  1 +
 kernel/config/builtin-x86.lds  |  1 +
 kernel/config/smp-x86.lds      |  1 +
 kernel/include/syscall.h       |  2 +-
 kernel/syscall.c               |  4 --
 7 files changed, 78 insertions(+), 8 deletions(-)

diff --git a/arch/x86/kernel/vsyscall.c b/arch/x86/kernel/vsyscall.c
index 40cfe21d..1813a455 100644
--- a/arch/x86/kernel/vsyscall.c
+++ b/arch/x86/kernel/vsyscall.c
@@ -18,14 +18,84 @@
  */
 
 #include <syscall.h>
+#include <ihk/atomic.h>
+#include <arch/cpu.h>
 
-extern int vsyscall_gettimeofday(void *tv, void *tz)
+extern int vsyscall_gettimeofday(struct timeval *tv, void *tz)
 	__attribute__ ((section (".vsyscall.gettimeofday")));
 
-int vsyscall_gettimeofday(void *tv, void *tz)
+struct tod_data_s tod_data
+		__attribute__ ((section(".vsyscall.gettimeofday.data"))) = {
+	.do_local =	0,
+	.version =	IHK_ATOMIC64_INIT(0),
+};
+
+static inline void cpu_pause_for_vsyscall(void)
+{
+	asm volatile ("pause" ::: "memory");
+	return;
+} /* cpu_pause_for_vsyscall() */
+
+static inline void calculate_time_from_tsc(struct timespec *ts)
+{
+	long ver;
+	unsigned long current_tsc;
+	__time_t sec_delta;
+	long ns_delta;
+
+	for (;;) {
+		while ((ver = ihk_atomic64_read(&tod_data.version)) & 1) {
+			/* settimeofday() is in progress */
+			cpu_pause_for_vsyscall();
+		}
+		rmb();
+		*ts = tod_data.origin;
+		rmb();
+		if (ver == ihk_atomic64_read(&tod_data.version)) {
+			break;
+		}
+
+		/* settimeofday() has intervened */
+		cpu_pause_for_vsyscall();
+	}
+
+	current_tsc = rdtsc();
+	sec_delta = current_tsc / tod_data.clocks_per_sec;
+	ns_delta = NS_PER_SEC * (current_tsc % tod_data.clocks_per_sec)
+		/ tod_data.clocks_per_sec;
+	/* calc. of ns_delta overflows if clocks_per_sec exceeds 18.44 GHz */
+
+	ts->tv_sec += sec_delta;
+	ts->tv_nsec += ns_delta;
+	if (ts->tv_nsec >= NS_PER_SEC) {
+		ts->tv_nsec -= NS_PER_SEC;
+		++ts->tv_sec;
+	}
+
+	return;
+} /* calculate_time_from_tsc() */
+
+int vsyscall_gettimeofday(struct timeval *tv, void *tz)
 {
 	int error;
+	struct timespec ats;
 
+	if (!tv && !tz) {
+		/* nothing to do */
+		return 0;
+	}
+
+	/* Do it locally if supported */
+	if (!tz && tod_data.do_local) {
+		calculate_time_from_tsc(&ats);
+
+		tv->tv_sec = ats.tv_sec;
+		tv->tv_usec = ats.tv_nsec / 1000;
+
+		return 0;
+	}
+
+	/* Otherwise syscall */
 	asm ("syscall" : "=a" (error)
 			: "a" (__NR_gettimeofday), "D" (tv), "S" (tz)
 			: "%rcx", "%r11", "memory");
@@ -34,7 +104,7 @@ int vsyscall_gettimeofday(void *tv, void *tz)
 		*(int *)0 = 0;	/* i.e. raise(SIGSEGV) */
 	}
 	return error;
-}
+} /* vsyscall_gettimeofday() */
 
 extern long vsyscall_time(void *tp)
 	__attribute__ ((section (".vsyscall.time")));
diff --git a/kernel/config/attached-mic.lds b/kernel/config/attached-mic.lds
index 3390a869..4da8397a 100644
--- a/kernel/config/attached-mic.lds
+++ b/kernel/config/attached-mic.lds
@@ -26,6 +26,7 @@ SECTIONS
 
 		. = vsyscall_page + 0x000;
 		*(.vsyscall.gettimeofday)
+		*(.vsyscall.gettimeofday.*)
 
 		. = vsyscall_page + 0x400;
 		*(.vsyscall.time)
diff --git a/kernel/config/builtin-mic.lds b/kernel/config/builtin-mic.lds
index 3390a869..4da8397a 100644
--- a/kernel/config/builtin-mic.lds
+++ b/kernel/config/builtin-mic.lds
@@ -26,6 +26,7 @@ SECTIONS
 
 		. = vsyscall_page + 0x000;
 		*(.vsyscall.gettimeofday)
+		*(.vsyscall.gettimeofday.*)
 
 		. = vsyscall_page + 0x400;
 		*(.vsyscall.time)
diff --git a/kernel/config/builtin-x86.lds b/kernel/config/builtin-x86.lds
index 1061fba9..bae71899 100644
--- a/kernel/config/builtin-x86.lds
+++ b/kernel/config/builtin-x86.lds
@@ -26,6 +26,7 @@ SECTIONS
 
 		. = vsyscall_page + 0x000;
 		*(.vsyscall.gettimeofday)
+		*(.vsyscall.gettimeofday.*)
 
 		. = vsyscall_page + 0x400;
 		*(.vsyscall.time)
diff --git a/kernel/config/smp-x86.lds b/kernel/config/smp-x86.lds
index 7fb32c8a..9f4b0832 100644
--- a/kernel/config/smp-x86.lds
+++ b/kernel/config/smp-x86.lds
@@ -26,6 +26,7 @@ SECTIONS
 
 		. = vsyscall_page + 0x000;
 		*(.vsyscall.gettimeofday)
+		*(.vsyscall.gettimeofday.*)
 
 		. = vsyscall_page + 0x400;
 		*(.vsyscall.time)
diff --git a/kernel/include/syscall.h b/kernel/include/syscall.h
index ebd58ead..4290cc50 100644
--- a/kernel/include/syscall.h
+++ b/kernel/include/syscall.h
@@ -296,6 +296,6 @@ struct tod_data_s {
 	unsigned long clocks_per_sec;
 	struct timespec origin;		/* realtime when tsc=0 */
 };
-extern struct tod_data_s tod_data;
+extern struct tod_data_s tod_data;	/* residing in arch-dependent file */
 
 #endif
diff --git a/kernel/syscall.c b/kernel/syscall.c
index 6a319d52..964f6b4a 100644
--- a/kernel/syscall.c
+++ b/kernel/syscall.c
@@ -95,10 +95,6 @@ static char *syscall_name[] MCKERNEL_UNUSED = {
 #undef	SYSCALL_DELEGATED
 };
 
-struct tod_data_s tod_data = {
-	.do_local =	0,
-	.version =	IHK_ATOMIC64_INIT(0),
-};
 static ihk_spinlock_t tod_data_lock = SPIN_LOCK_UNLOCKED;
 
 void check_signal(unsigned long, void *, int);