clock driver etc

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

1 files changed, 76 insertions, 4 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()