From 6faddda1a4d0e6e5f75e1abc1fcd0c5541f78589 Mon Sep 17 00:00:00 2001
From: Martin Lucina <martin@lucina.net>
Date: Thu, 25 Jun 2015 11:06:37 +0200
Subject: [PATCH] hw: Calibrate TSC and correctly scale monotonic time

This is the first phase of addressing issue #30. We calibrate the TSC at
boot against the i8254 timer and calculate a scaleing factor for
monotonic time.
---
 platform/hw/arch/x86/clock.c       | 117 +++++++++++++++++++++++++----
 platform/hw/include/arch/x86/reg.h |   3 +
 platform/hw/include/bmk/kernel.h   |   1 +
 platform/hw/kernel.c               |   2 +-
 4 files changed, 109 insertions(+), 14 deletions(-)

diff --git a/platform/hw/arch/x86/clock.c b/platform/hw/arch/x86/clock.c
index 398065651..1819be95d 100644
--- a/platform/hw/arch/x86/clock.c
+++ b/platform/hw/arch/x86/clock.c
@@ -1,5 +1,6 @@
 /*-
  * Copyright (c) 2014, 2015 Antti Kantee.  All Rights Reserved.
+ * Copyright (c) 2015 Martin Lucina.  All Rights Reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -25,12 +26,73 @@
 
 #include <bmk/kernel.h>
 
+#define NSEC_PER_SEC	1000000000ULL
+#define TSC_SHIFT	27
+
 /* clock isr trampoline (in locore.S) */
 void bmk_cpu_isr_clock(void);
 
+/* TSC multiplier for converting ticks to nsecs, scaled by TSC_SHIFT. */
+static uint64_t tsc_mult;
+
+/* Base time values at the last call to bmk_cpu_clock_now(). */
+static bmk_time_t time_base;
+static uint64_t tsc_base;
+
+/*
+ * Read the current i8254 channel 0 tick count.
+ */
+static unsigned int
+i8254_gettick(void)
+{
+	uint16_t rdval;
+
+	outb(TIMER_MODE, TIMER_SEL0 | TIMER_LATCH);
+	rdval = inb(TIMER_CNTR);
+	rdval |= (inb(TIMER_CNTR) << 8);
+	return rdval;
+}
+
+/*
+ * Delay for approximately n microseconds using the i8254 channel 0 counter.
+ * Timer must be programmed appropriately before calling this function.
+ */
+static void
+i8254_delay(unsigned int n)
+{
+	unsigned int cur_tick, initial_tick;
+	int remaining;
+	const unsigned long timer_rval = TIMER_HZ / HZ;
+
+	initial_tick = i8254_gettick();
+
+	remaining = (unsigned long long) n * TIMER_HZ / 1000000;
+
+	while (remaining > 1) {
+		cur_tick = i8254_gettick();
+		if (cur_tick > initial_tick)
+			remaining -= timer_rval - (cur_tick - initial_tick);
+		else
+			remaining -= initial_tick - cur_tick;
+		initial_tick = cur_tick;
+	}
+}
+
+static uint64_t
+rdtsc(void)
+{
+	uint64_t val;
+	unsigned long eax, edx;
+
+	__asm__ __volatile__("rdtsc" : "=a"(eax), "=d"(edx));
+	val = ((uint64_t)edx<<32)|(eax);
+	return val;
+}
+
 void
 bmk_x86_initclocks(void)
 {
+	uint64_t tsc_freq;
 
 	/*
 	 * map clock interrupt.
@@ -39,31 +101,60 @@ bmk_x86_initclocks(void)
 	 */
 	bmk_x86_fillgate(32, bmk_cpu_isr_clock, 0);
 
-	/* initialize the timer to 100Hz */
-	outb(TIMER_MODE, TIMER_RATEGEN | TIMER_16BIT);
-	outb(TIMER_CNTR, TIMER_HZ/HZ & 0xff);
-	outb(TIMER_CNTR, TIMER_HZ/HZ >> 8);
+	/* Initialise i8254 timer channel 0 to mode 2 at 100 Hz */
+	outb(TIMER_MODE, TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
+	outb(TIMER_CNTR, (TIMER_HZ / HZ) & 0xff);
+	outb(TIMER_CNTR, (TIMER_HZ / HZ) >> 8);
+
+	/*
+	 * Calculate TSC frequency by calibrating against an 0.1s delay
+	 * using the i8254 timer.
+	 */
+	tsc_base = rdtsc();
+	i8254_delay(100000);
+	tsc_freq = (rdtsc() - tsc_base) * 10;
+
+	/*
+	 * Calculate TSC scaling multiplier and initialiase time_base.
+	 */
+	tsc_mult = (NSEC_PER_SEC << TSC_SHIFT) / tsc_freq;
+	time_base = (tsc_base * tsc_mult) >> TSC_SHIFT;
 }
 
 void
 bmk_isr_clock(void)
 {
 
-	/* nada */
+	/*
+	 * Nothing to do here, clock interrupt serves only as a way to wake
+	 * the CPU from halt state.
+	 */
 }
 
 bmk_time_t
 bmk_cpu_clock_now(void)
 {
-	uint64_t val;
-	unsigned long eax, edx;
+	uint64_t tsc_now, tsc_delta;
 
-	/* um um um */
-	__asm__ __volatile__("rdtsc" : "=a"(eax), "=d"(edx));
-	val = ((uint64_t)edx<<32)|(eax);
+	/*
+	 * Update time_base (monotonic time) and tsc_base (TSC time).
+	 * Ensure to use a delta between now and the last call to
+	 * bmk_cpu_clock_now() to prevent overflow.
+	 * XXX: Document when overflow would happen.
+	 */
+	tsc_now = rdtsc();
+	tsc_delta = tsc_now - tsc_base;
+	time_base += (tsc_delta * tsc_mult) >> TSC_SHIFT;
+	tsc_base = tsc_now;
 
-	/* just approximate that 1 cycle = 1ns.  "good enuf" for now */
-	return val;
+	return time_base;
+}
+
+void
+bmk_cpu_block(bmk_time_t until)
+{
+
+	bmk_cpu_nanohlt();
 }
 
 void
@@ -71,7 +162,7 @@ bmk_cpu_nanohlt(void)
 {
 
 	/*
-	 * Enable clock interrupt and wait for the next whichever interrupt
+	 * Enable clock interrupt and wait for the next whichever interrupt.
 	 */
 	outb(PIC1_DATA, 0xff & ~(1<<2|1<<0));
 	hlt();
diff --git a/platform/hw/include/arch/x86/reg.h b/platform/hw/include/arch/x86/reg.h
index 973695bf0..5a27eff34 100644
--- a/platform/hw/include/arch/x86/reg.h
+++ b/platform/hw/include/arch/x86/reg.h
@@ -8,6 +8,9 @@
 
 #define TIMER_CNTR	0x40
 #define TIMER_MODE	0x43
+#define TIMER_SEL0	0x00
+#define TIMER_LATCH	0x00
 #define TIMER_RATEGEN	0x04
+#define TIMER_ONESHOT	0x08
 #define TIMER_16BIT	0x30
 #define TIMER_HZ	1193182
diff --git a/platform/hw/include/bmk/kernel.h b/platform/hw/include/bmk/kernel.h
index 232ce52cf..072182317 100644
--- a/platform/hw/include/bmk/kernel.h
+++ b/platform/hw/include/bmk/kernel.h
@@ -16,6 +16,7 @@ void bmk_cons_putc(int);
 void bmk_cons_puts(const char *);
 
 void bmk_cpu_init(void);
+void bmk_cpu_block(bmk_time_t);
 void bmk_cpu_nanohlt(void);
 int bmk_cpu_intr_init(int);
 void bmk_cpu_intr_ack(void);
diff --git a/platform/hw/kernel.c b/platform/hw/kernel.c
index 20ad79b14..b6072851f 100644
--- a/platform/hw/kernel.c
+++ b/platform/hw/kernel.c
@@ -46,7 +46,7 @@ bmk_platform_block(bmk_time_t until)
 		bmk_spldepth = 1;
 		spl0();
 	}
-	bmk_cpu_nanohlt();
+	bmk_cpu_block(until);
 	if (s) {
 		splhigh();
 		bmk_spldepth = s;