Tests for kvmclock

index d0d3df2..9cb90cf 100644
@@ -23,10 +23,14 @@
 #include <stdarg.h>
 
 typedef unsigned char u8;
+typedef signed char s8;
 typedef unsigned short u16;
+typedef signed short s16;
 typedef unsigned u32;
+typedef signed s32;
 typedef unsigned long ulong;
 typedef unsigned long long u64;
+typedef signed long long s64;
 typedef unsigned long size_t;
 typedef _Bool bool;
 

index c2e7350..df5fdba 100644
@@ -1,6 +1,10 @@
 #ifndef __SMP_H
 #define __SMP_H
 
+#define mb() 	asm volatile("mfence":::"memory")
+#define rmb()	asm volatile("lfence":::"memory")
+#define wmb()	asm volatile("sfence" ::: "memory")
+
 struct spinlock {
     int v;
 };

new file mode 100644
index 0000000..da74ff2
@@ -0,0 +1,37 @@
+#include <libcflat.h>
+#include "atomic.h"
+
+#ifdef __i386__
+
+u64 atomic64_cmpxchg(atomic64_t *v, u64 old, u64 new)
+{
+        u32 low = new;
+        u32 high = new >> 32;
+
+        asm volatile("lock cmpxchg8b %1\n"
+                     : "+A" (old),
+                       "+m" (*(volatile long long *)&v->counter)
+                     : "b" (low), "c" (high)
+                     : "memory"
+                     );
+
+        return old;
+}
+
+#else
+
+u64 atomic64_cmpxchg(atomic64_t *v, u64 old, u64 new)
+{
+        u64 ret;
+        u64 _old = old;
+        u64 _new = new;
+
+        asm volatile("lock cmpxchgq %2,%1"
+                     : "=a" (ret), "+m" (*(volatile long *)&v->counter)
+                     : "r" (_new), "0" (_old)
+                     : "memory"
+                     );
+        return ret;
+}
+
+#endif

164 lines not on this page · raw message

index 67d7ca5..c348808 100644
@@ -258,4 +258,26 @@ static inline void sti(void)
     asm volatile ("sti");
 }
 
+static inline unsigned long long rdtsc()
+{
+	long long r;
+
+#ifdef __x86_64__
+	unsigned a, d;
+
+	asm volatile ("rdtsc" : "=a"(a), "=d"(d));
+	r = a | ((long long)d << 32);
+#else
+	asm volatile ("rdtsc" : "=A"(r));
+#endif
+	return r;
+}
+
+static inline void wrtsc(u64 tsc)
+{
+	unsigned a = tsc, d = tsc >> 32;
+
+	asm volatile("wrmsr" : : "a"(a), "d"(d), "c"(0x10));
+}
+
 #endif

index 394a9c6..58f332d 100644
@@ -1,19 +1,5 @@
 #include "libcflat.h"
-
-u64 rdtsc(void)
-{
-	unsigned a, d;
-
-	asm volatile("rdtsc" : "=a"(a), "=d"(d));
-	return a | (u64)d << 32;
-}
-
-void wrtsc(u64 tsc)
-{
-	unsigned a = tsc, d = tsc >> 32;
-
-	asm volatile("wrmsr" : : "a"(a), "d"(d), "c"(0x10));
-}
+#include "processor.h"
 
 void test_wrtsc(u64 t1)
 {

index 84c384d..551083d 100644
@@ -3,21 +3,6 @@
 #include "smp.h"
 #include "processor.h"
 
-static inline unsigned long long rdtsc()
-{
-	long long r;
-
-#ifdef __x86_64__
-	unsigned a, d;
-
-	asm volatile ("rdtsc" : "=a"(a), "=d"(d));
-	r = a | ((long long)d << 32);
-#else
-	asm volatile ("rdtsc" : "=A"(r));
-#endif
-	return r;
-}
-
 static unsigned int inl(unsigned short port)
 {
     unsigned int val;

index 9cb90cf..d4ee2e0 100644
@@ -50,6 +50,7 @@ extern void puts(const char *s);
 
 extern void *memset(void *s, int c, size_t n);
 
+extern long atol(const char *ptr);
 #define ARRAY_SIZE(_a)  (sizeof(_a)/sizeof((_a)[0]))
 
 #endif

index acac3c0..1f19f5c 100644
@@ -30,3 +30,34 @@ void *memset(void *s, int c, size_t n)
 
     return s;
 }
+
+long atol(const char *ptr)
+{
+    long acc = 0;
+    const char *s = ptr;
+    int neg, c;
+
+    while (*s == ' ' || *s == '\t')
+        s++;
+    if (*s == '-'){
+        neg = 1;
+        s++;
+    } else {
+        neg = 0;
+        if (*s == '+')
+            s++;
+    }
+
+    while (*s) {
+        if (*s < '0' || *s > '9')
+            break;
+        c = *s - '0';
+        acc = acc * 10 + c;
+        s++;
+    }
+
+    if (neg)
+        acc = -acc;
+
+    return acc;
+}

new file mode 100644
index 0000000..0624da3
@@ -0,0 +1,279 @@
+#include "libcflat.h"
+#include "smp.h"
+#include "atomic.h"
+#include "processor.h"
+#include "kvmclock.h"
+
+#define unlikely(x)	__builtin_expect(!!(x), 0)
+#define likely(x)	__builtin_expect(!!(x), 1)
+
+
+struct pvclock_vcpu_time_info __attribute__((aligned(4))) hv_clock[MAX_CPU];
+struct pvclock_wall_clock wall_clock;
+static unsigned char valid_flags = 0;
+static atomic64_t last_value = ATOMIC64_INIT(0);
+
+/*
+ * Scale a 64-bit delta by scaling and multiplying by a 32-bit fraction,
+ * yielding a 64-bit result.
+ */
+static inline u64 scale_delta(u64 delta, u32 mul_frac, int shift)
+{
+	u64 product;
+#ifdef __i386__
+	u32 tmp1, tmp2;
+#endif
+
+	if (shift < 0)
+		delta >>= -shift;
+	else
+		delta <<= shift;
+
+#ifdef __i386__
+	__asm__ (
+		"mul  %5       ; "
+		"mov  %4,%%eax ; "
+		"mov  %%edx,%4 ; "
+		"mul  %5       ; "
+		"xor  %5,%5    ; "
+		"add  %4,%%eax ; "
+		"adc  %5,%%edx ; "
+		: "=A" (product), "=r" (tmp1), "=r" (tmp2)
+		: "a" ((u32)delta), "1" ((u32)(delta >> 32)), "2" (mul_frac) );
+#elif defined(__x86_64__)
+	__asm__ (
+		"mul %%rdx ; shrd $32,%%rdx,%%rax"
+		: "=a" (product) : "0" (delta), "d" ((u64)mul_frac) );
+#else
+#error implement me!
+#endif
+
+	return product;
+}
+
+#ifdef __i386__
+# define do_div(n,base) ({					\
+	u32 __base = (base);    				\
+	u32 __rem;						\
+	__rem = ((u64)(n)) % __base;                            \
+	(n) = ((u64)(n)) / __base;				\
+	__rem;							\
+ })
+#else
+u32 __attribute__((weak)) __div64_32(u64 *n, u32 base)
+{
+	u64 rem = *n;
+	u64 b = base;
+	u64 res, d = 1;
+	u32 high = rem >> 32;
+
+	/* Reduce the thing a bit first */
+	res = 0;
+	if (high >= base) {
+		high /= base;
+		res = (u64) high << 32;
+		rem -= (u64) (high*base) << 32;
+	}
+
+	while ((s64)b > 0 && b < rem) {
+		b = b+b;
+		d = d+d;
+	}
+
+	do {
+		if (rem >= b) {
+			rem -= b;
+			res += d;
+		}
+		b >>= 1;
+		d >>= 1;
+	} while (d);
+
+	*n = res;
+	return rem;
+}
+
+# define do_div(n,base) ({				\
+	u32 __base = (base);    			\
+	u32 __rem;					\
+	(void)(((typeof((n)) *)0) == ((u64 *)0));	\
+	if (likely(((n) >> 32) == 0)) {			\
+		__rem = (u32)(n) % __base;		\
+		(n) = (u32)(n) / __base;		\
+	} else 						\
+		__rem = __div64_32(&(n), __base);	\
+	__rem;						\
+ })
+#endif
+
+/**
+ * set_normalized_timespec - set timespec sec and nsec parts and normalize
+ *
+ * @ts:		pointer to timespec variable to be set
+ * @sec:	seconds to set
+ * @nsec:	nanoseconds to set
+ *
+ * Set seconds and nanoseconds field of a timespec variable and
+ * normalize to the timespec storage format
+ *
+ * Note: The tv_nsec part is always in the range of
+ *	0 <= tv_nsec < NSEC_PER_SEC
+ * For negative values only the tv_sec field is negative !
+ */
+void set_normalized_timespec(struct timespec *ts, long sec, s64 nsec)
+{
+	while (nsec >= NSEC_PER_SEC) {
+		/*
+		 * The following asm() prevents the compiler from
+		 * optimising this loop into a modulo operation. See
+		 * also __iter_div_u64_rem() in include/linux/time.h
+		 */
+		asm("" : "+rm"(nsec));
+		nsec -= NSEC_PER_SEC;
+		++sec;
+	}
+	while (nsec < 0) {
+		asm("" : "+rm"(nsec));
+		nsec += NSEC_PER_SEC;
+		--sec;
+	}
+	ts->tv_sec = sec;
+	ts->tv_nsec = nsec;
+}
+
+static u64 pvclock_get_nsec_offset(struct pvclock_shadow_time *shadow)
+{
+	u64 delta = rdtsc() - shadow->tsc_timestamp;
+	return scale_delta(delta, shadow->tsc_to_nsec_mul, shadow->tsc_shift);
+}
+
+/*
+ * Reads a consistent set of time-base values from hypervisor,
+ * into a shadow data area.
+ */
+static unsigned pvclock_get_time_values(struct pvclock_shadow_time *dst,
+					struct pvclock_vcpu_time_info *src)
+{
+	do {
+		dst->version = src->version;
+		rmb();		/* fetch version before data */
+		dst->tsc_timestamp     = src->tsc_timestamp;
+		dst->system_timestamp  = src->system_time;
+		dst->tsc_to_nsec_mul   = src->tsc_to_system_mul;
+		dst->tsc_shift         = src->tsc_shift;
+		dst->flags             = src->flags;
+		rmb();		/* test version after fetching data */
+	} while ((src->version & 1) || (dst->version != src->version));
+
+	return dst->version;
+}
+
+cycle_t pvclock_clocksource_read(struct pvclock_vcpu_time_info *src)
+{
+	struct pvclock_shadow_time shadow;
+	unsigned version;
+	cycle_t ret, offset;
+	u64 last;
+
+	do {
+		version = pvclock_get_time_values(&shadow, src);
+		barrier();
+		offset = pvclock_get_nsec_offset(&shadow);
+		ret = shadow.system_timestamp + offset;
+		barrier();
+	} while (version != src->version);
+
+	if ((valid_flags & PVCLOCK_RAW_CYCLE_BIT) ||
+            ((valid_flags & PVCLOCK_TSC_STABLE_BIT) &&
+             (shadow.flags & PVCLOCK_TSC_STABLE_BIT)))
+                return ret;
+
+	/*
+	 * Assumption here is that last_value, a global accumulator, always goes
+	 * forward. If we are less than that, we should not be much smaller.
+	 * We assume there is an error marging we're inside, and then the correction
+	 * does not sacrifice accuracy.
+	 *
+	 * For reads: global may have changed between test and return,
+	 * but this means someone else updated poked the clock at a later time.
+	 * We just need to make sure we are not seeing a backwards event.
+	 *
+	 * For updates: last_value = ret is not enough, since two vcpus could be
+	 * updating at the same time, and one of them could be slightly behind,
+	 * making the assumption that last_value always go forward fail to hold.
+	 */
+	last = atomic64_read(&last_value);
+	do {
+		if (ret < last)
+			return last;
+		last = atomic64_cmpxchg(&last_value, last, ret);
+	} while (unlikely(last != ret));
+
+	return ret;
+}
+
+cycle_t kvm_clock_read()
+{
+        struct pvclock_vcpu_time_info *src;
+        cycle_t ret;
+        int index = smp_id();
+
+        src = &hv_clock[index];
+        ret = pvclock_clocksource_read(src);
+        return ret;
+}
+
+void kvm_clock_init(void *data)
+{
+        int index = smp_id();
+        struct pvclock_vcpu_time_info *hvc = &hv_clock[index];
+
+        printf("kvm-clock: cpu %d, msr 0x:%lx \n", index, hvc);
+        wrmsr(MSR_KVM_SYSTEM_TIME, (unsigned long)hvc | 1);
+}
+
+void kvm_clock_clear(void *data)
+{
+        wrmsr(MSR_KVM_SYSTEM_TIME, 0LL);
+}
+
+void pvclock_read_wallclock(struct pvclock_wall_clock *wall_clock,
+			    struct pvclock_vcpu_time_info *vcpu_time,
+			    struct timespec *ts)
+{
+	u32 version;
+	u64 delta;
+	struct timespec now;
+
+	/* get wallclock at system boot */
+	do {
+		version = wall_clock->version;
+		rmb();		/* fetch version before time */
+		now.tv_sec  = wall_clock->sec;
+		now.tv_nsec = wall_clock->nsec;
+		rmb();		/* fetch time before checking version */
+	} while ((wall_clock->version & 1) || (version != wall_clock->version));
+
+	delta = pvclock_clocksource_read(vcpu_time);	/* time since system boot */
+	delta += now.tv_sec * (u64)NSEC_PER_SEC + now.tv_nsec;
+
+	now.tv_nsec = do_div(delta, NSEC_PER_SEC);
+	now.tv_sec = delta;
+
+	set_normalized_timespec(ts, now.tv_sec, now.tv_nsec);
+}
+
+void kvm_get_wallclock(struct timespec *ts)
+{
+        struct pvclock_vcpu_time_info *vcpu_time;
+        int index = smp_id();
+
+        wrmsr(MSR_KVM_WALL_CLOCK, (unsigned long)&wall_clock);
+        vcpu_time = &hv_clock[index];
+        pvclock_read_wallclock(&wall_clock, vcpu_time, ts);
+}
+
+void pvclock_set_flags(unsigned char flags)
+{
+        valid_flags = flags;
+}

60 lines not on this page · raw message

166 lines not on this page · raw message