diff options
| -rw-r--r-- | stage3/clock.c | 80 | ||||
| -rw-r--r-- | stage3/clock.h | 2 | ||||
| -rw-r--r-- | stage3/font.c | 16 | ||||
| -rw-r--r-- | stage3/font.h | 7 | ||||
| -rw-r--r-- | stage3/interrupts.c | 1 | ||||
| -rw-r--r-- | stage3/main.c | 1 | ||||
| -rw-r--r-- | stage3/thread.c | 2 |
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(); |