clock driver etc

fancier monotonic clock: RTC interpolated with TSC
font cursor position access APIs
remove silly thread resume in main()

7 files changed, 104 insertions, 5 deletions

diff --git a/stage3/clock.c b/stage3/clock.c
index 372d3da..2513277 100644
--- a/stage3/clock.c
+++ b/stage3/clock.c
@@ -6,7 +6,25 @@
 #include "heap.h"
 #include "halt.h"
 
+#define SYNC_INTERVAL NANOSECONDS/10
+
 u64 monoclock_rtc_time = 0;
+u64 monoclock_last_cycles = 0;
+
+u64 hd_time = 0;
+u64 last_cycles = 0;
+u64 last_hd_time = 0;
+u64 last_sync = 0;
+u64 last_ssync = 0;
+#define RING 10
+u64 ring_pos = 0;
+u64 last_cycles_ring[RING] = {0};
+u64 last_sync_ring[RING] = {0};
+double hd_drift = 0;
+double hd_rate = 1e10000;
+
+#define ADJ_TARGET SYNC_INTERVAL*RING
+#define MAX_OVERSHOOT ADJ_TARGET*2
 
 void clock_init()
 {
@@ -17,13 +35,67 @@ void clock_init()
 	unmask_irq(8);
 }
 
-u64 clock_monotonic_coarse() {
-	return monoclock_rtc_time;
+static inline u64 monoclock(u64 last_cycles, u64 last_hd_time, u64 max_overshoot)
+{
+	double fcycdiff = clock_cycles() - last_cycles;
+	fcycdiff *= hd_drift;
+	fcycdiff /= hd_rate;
+	if (fcycdiff < 0)
+		fcycdiff = 0;
+	u64 cycdiff = fcycdiff;
+	if (cycdiff > max_overshoot)
+		cycdiff = max_overshoot;
+	return last_hd_time + cycdiff;
 }
 
-u64 clock_monotonic()
+void clock_sync()
 {
-	return monoclock_rtc_time; // TODO: high resolution clock
+	u64 monotime = monoclock(last_cycles, last_hd_time, MAX_OVERSHOOT);
+	u64 cycles = clock_cycles();
+	u64 rtc_time = monoclock_rtc_time;
+	if (last_sync + SYNC_INTERVAL <= rtc_time) {
+		u64 ring_next = (ring_pos + 1) % RING;
+		u64 diff = rtc_time - last_sync_ring[ring_next];
+		last_sync = rtc_time;
+		last_cycles = cycles;
+		last_hd_time = monotime;
+		last_sync_ring[ring_pos] = rtc_time;
+		last_cycles_ring[ring_pos] = cycles;
+		hd_rate = cycles - last_cycles_ring[ring_next];
+		hd_drift = (i64)(last_sync) + (i64)(ADJ_TARGET) - (i64)(monotime);
+		ring_pos = ring_next;
+#ifdef MONOCLOCK_DEBUG
+		term_pos old_cursor = font_get_cursor();
+		font_set_cursor((term_pos){0,0});
+		print(S("RTC: "));
+		print_num(monoclock_rtc_time,10);
+		print(S("    \nUSR: "));
+		print_num(clock_monotonic(),10);
+		print(S("    \nADJ: "));
+		print_num(monotime,10);
+		print(S("    \nTSC: "));
+		print_num(cycles,10);
+		print(S("    \ndist:  "));
+		print_num(diff,10);
+		print(S("    \nrate : "));
+		print_dbl(hd_rate,4);
+		print(S("    \ndrift: "));
+		print_dbl(hd_drift,4);
+		print(S("    \nadj: "));
+		print_dbl(hd_drift/hd_rate,4);
+		print(S("    "));
+		font_set_cursor(old_cursor);
+#endif
+	}
+}
+
+u64 clock_monotonic_coarse()
+{
+	return monoclock_rtc_time;
+}
+
+u64 clock_monotonic() {
+	return monoclock(monoclock_last_cycles, monoclock_rtc_time, RTC_RATE);
 }
 
 u64 clock_cycles()
diff --git a/stage3/clock.h b/stage3/clock.h
index f0255bc..1c6f817 100644
--- a/stage3/clock.h
+++ b/stage3/clock.h
@@ -8,8 +8,10 @@
 #define RTC_RATE NANOSECONDS/RTC_FREQ
 
 extern u64 monoclock_rtc_time;
+extern u64 monoclock_last_cycles;
 
 void clock_init();
+void clock_sync();
 
 u64 clock_cycles();
 u64 clock_monotonic_coarse();
diff --git a/stage3/font.c b/stage3/font.c
index 3bbba86..4fd0c1d 100644
--- a/stage3/font.c
+++ b/stage3/font.c
@@ -38,6 +38,22 @@ void font_set_size(u16 size)
 	screen_height = gfx_info->height / outer_height;
 }
 
+void font_set_cursor(term_pos new_cursor)
+{
+	cursor_x = new_cursor.x;
+	cursor_y = new_cursor.y;
+}
+
+term_pos font_get_cursor()
+{
+	return (term_pos){cursor_x, cursor_y};
+}
+
+term_pos font_get_size()
+{
+	return (term_pos){screen_width, screen_height};
+}
+
 void font_load_blob(const void *blob)
 {
 	memcpy(font, blob, 256*16);
diff --git a/stage3/font.h b/stage3/font.h
index 03772db..44019bb 100644
--- a/stage3/font.h
+++ b/stage3/font.h
@@ -3,8 +3,15 @@
 
 #include "def.h"
 
+typedef struct {
+	u16 x,y;
+} term_pos;
+
 void font_init();
 void font_set_size(u16 size);
+void font_set_cursor(term_pos new_cursor);
+term_pos font_get_cursor();
+term_pos font_get_size();
 void font_load_blob(const void *blob);
 void font_load_classic();
 void font_load_builtin();
diff --git a/stage3/interrupts.c b/stage3/interrupts.c
index 70b3e4a..e2dd148 100644
--- a/stage3/interrupts.c
+++ b/stage3/interrupts.c
@@ -201,6 +201,7 @@ void interrupt_handler(interrupt_frame *frame)
 			outb(0x70, 0x0C);
 			inb(0x71);
 			monoclock_rtc_time += RTC_RATE;
+			monoclock_last_cycles = clock_cycles();
 		} else {
 			if (queue_write.len == queue_write.cap) {
 				panic(S("queue exceeded\n"));
diff --git a/stage3/main.c b/stage3/main.c
index a30c3b9..fd18d6c 100644
--- a/stage3/main.c
+++ b/stage3/main.c
@@ -145,7 +145,6 @@ void kmain()
 	unmask_irq(1);
 	unmask_irq(2);
 	clock_init();
-	thread_resume(nil, keyboard_thread);
 	enable_irqs();
 
 	thread_sched(nil, nil);
diff --git a/stage3/thread.c b/stage3/thread.c
index aeec3bd..4609102 100644
--- a/stage3/thread.c
+++ b/stage3/thread.c
@@ -1,6 +1,7 @@
 #include "thread.h"
 #include "heap.h"
 #include "pic.h"
+#include "clock.h"
 
 static thread *current_thread = nil;
 void *thread_sched_stack = nil;
@@ -56,6 +57,7 @@ void thread_sched(yield_arg *arg, void *stack)
 	}
 
 	for (;;) {
+		clock_sync();
 		if (queue_read.len == 0) {
 			disable_irqs();