7 files changed, 143 insertions, 1 deletions

diff --git a/kernel/src/arch/aarch64/aarch64.cpp b/kernel/src/arch/aarch64/aarch64.cpp
index 52003f8..72f6c09 100644
--- a/kernel/src/arch/aarch64/aarch64.cpp
+++ b/kernel/src/arch/aarch64/aarch64.cpp
@@ -56,6 +56,15 @@ namespace arch {
         asm volatile("" ::: "memory"); 
     }
 
+    [[gnu::weak]] bool test_interrupts() {
+        uint64_t daif;
+        __asm__ __volatile__ (
+            "mrs %0, daif"
+            : "=r" (daif)
+        );
+        return (daif & (1 << 7)) == 0;
+    }
+
     [[gnu::weak]] void init(int stage) {
         switch(stage) {
         case ARCH_INIT_EARLY:
diff --git a/kernel/src/arch/riscv64/riscv64.cpp b/kernel/src/arch/riscv64/riscv64.cpp
index 784cc94..8e2fb37 100644
--- a/kernel/src/arch/riscv64/riscv64.cpp
+++ b/kernel/src/arch/riscv64/riscv64.cpp
@@ -20,6 +20,16 @@ namespace arch {
         asm volatile("" ::: "memory"); 
     }
 
+    [[gnu::weak]] bool test_interrupts() {
+        uint64_t status;
+        __asm__ __volatile__ (
+            "csrr %0, mstatus" 
+            : "=r" (status)
+        );
+
+        return (status & (1 << 3)) != 0;
+    }
+
     [[gnu::weak]] void hcf() {
         disable_interrupts();
         while(true) {
diff --git a/kernel/src/arch/x86_64/cpu_local.hpp b/kernel/src/arch/x86_64/cpu_local.hpp
index e839413..d5add64 100644
--- a/kernel/src/arch/x86_64/cpu_local.hpp
+++ b/kernel/src/arch/x86_64/cpu_local.hpp
@@ -15,6 +15,7 @@ typedef struct {
     std::uint64_t timer_ist_stack;
     std::uint32_t cpu;
     std::uint64_t tsc_freq;
+    void* pvclock_buffer;
     thread* current_thread;
 } cpudata_t;
 
diff --git a/kernel/src/arch/x86_64/drivers/pvclock.cpp b/kernel/src/arch/x86_64/drivers/pvclock.cpp
new file mode 100644
index 0000000..d77ef0f
--- /dev/null
+++ b/kernel/src/arch/x86_64/drivers/pvclock.cpp
@@ -0,0 +1,79 @@
+#define BILLION 1000000000UL
+#include <cstdint>
+#include <drivers/acpi.hpp>
+#include <arch/x86_64/drivers/pvclock.hpp>
+#include <arch/x86_64/cpu_local.hpp>
+#include <utils/gobject.hpp>
+#include <klibc/stdio.hpp>
+#include <arch/x86_64/assembly.hpp>
+#include <generic/pmm.hpp>
+#include <generic/hhdm.hpp>
+
+#include <generic/arch.hpp>
+
+void drivers::pvclock::init() {
+    std::uint32_t a,b,c,d;
+
+    assembly::cpuid(0x40000000,0,&a,&b,&c,&d);
+    if(b != 0x4b4d564b || d != 0x4d || c != 0x564b4d56)
+        return;
+
+    assembly::cpuid(0x40000001,0,&a,&b,&c,&d);
+    if(!(a & (1 << 3)))
+        return;
+
+    x86_64::cpu_data()->pvclock_buffer = (void*)(pmm::freelist::alloc_4k() + etc::hhdm());
+
+    assembly::wrmsr(0x4b564d01,((std::uint64_t)x86_64::cpu_data()->pvclock_buffer - etc::hhdm()) | 1);
+}
+
+void drivers::pvclock::bsp_init() {
+    std::uint32_t a,b,c,d;
+
+    assembly::cpuid(0x40000000,0,&a,&b,&c,&d);
+    if(b != 0x4b4d564b || d != 0x4d || c != 0x564b4d56)
+        return;
+
+    assembly::cpuid(0x40000001,0,&a,&b,&c,&d);
+    if(!(a & (1 << 3)))
+        return;
+
+    pvclock::init();
+
+    drivers::pvclock_timer* timer = new drivers::pvclock_timer;
+    time::setup_timer(timer);
+
+    klibc::printf("pvclock: pointer to buffer 0x%p\r\n",x86_64::cpu_data()->pvclock_buffer);
+}
+
+namespace drivers {
+    
+    std::uint64_t pvclock_timer::current_nano() {
+        uint32_t lo, hi;
+        asm volatile ("rdtsc" : "=a"(lo), "=d"(hi));
+        std::uint64_t tsc_v = ((uint64_t)hi << 32) | lo;
+        pvclock_vcpu_time_info* kvmclock_info = (pvclock_vcpu_time_info*)(x86_64::cpu_data()->pvclock_buffer);
+        std::uint64_t time0 = tsc_v - kvmclock_info->tsc_timestamp;
+        if(kvmclock_info->tsc_shift >= 0)
+            time0 <<= kvmclock_info->tsc_shift;
+        else
+            time0 >>= -kvmclock_info->tsc_shift;
+        time0 = (time0 * kvmclock_info->tsc_to_system_mul) >> 32;
+        time0 = time0 + kvmclock_info->system_time;
+        return time0;
+    }
+
+    void pvclock_timer::sleep(std::uint64_t us) {
+        std::uint64_t current = this->current_nano();
+        std::uint64_t end_ns = us * 1000;
+        std::uint64_t target = current + end_ns;
+        
+        while(this->current_nano() < target) {
+            arch::pause();
+        }
+    }
+
+    int pvclock_timer::get_priority() {
+        return 999999999; 
+    }
+}
\ No newline at end of file
diff --git a/kernel/src/arch/x86_64/drivers/pvclock.hpp b/kernel/src/arch/x86_64/drivers/pvclock.hpp
new file mode 100644
index 0000000..6881107
--- /dev/null
+++ b/kernel/src/arch/x86_64/drivers/pvclock.hpp
@@ -0,0 +1,30 @@
+#pragma once
+#include <cstdint>
+#include <generic/time.hpp>
+
+struct pvclock_vcpu_time_info {
+    std::uint32_t version;
+    std::uint32_t pad0;
+    std::uint64_t tsc_timestamp;
+    std::uint64_t system_time;
+    std::uint32_t tsc_to_system_mul;
+    std::int8_t tsc_shift;
+    std::uint8_t flags;
+    std::uint8_t pad[2];
+};
+
+namespace drivers {
+
+    class pvclock_timer final : public time::generic_timer {
+    public:
+        int get_priority() override;
+        std::uint64_t current_nano() override;
+        void sleep(std::uint64_t us) override;
+    };
+
+    class pvclock {
+    public:
+        static void init();
+        static void bsp_init();
+    };
+};
\ No newline at end of file
diff --git a/kernel/src/arch/x86_64/drivers/tsc.cpp b/kernel/src/arch/x86_64/drivers/tsc.cpp
index 0abcf6e..3776144 100644
--- a/kernel/src/arch/x86_64/drivers/tsc.cpp
+++ b/kernel/src/arch/x86_64/drivers/tsc.cpp
@@ -109,6 +109,6 @@ namespace drivers {
     }
 
     int tsc_timer::get_priority() {
-        return 9999999; // undertale reference
+        return 9999; 
     }
 }
\ No newline at end of file
diff --git a/kernel/src/arch/x86_64/x86_64.cpp b/kernel/src/arch/x86_64/x86_64.cpp
index 4d272da..1389351 100644
--- a/kernel/src/arch/x86_64/x86_64.cpp
+++ b/kernel/src/arch/x86_64/x86_64.cpp
@@ -15,6 +15,7 @@
 #include <arch/x86_64/schedule_timer.hpp>
 #include <utils/gobject.hpp>
 #include <arch/x86_64/cpu/sse.hpp>
+#include <arch/x86_64/drivers/pvclock.hpp>
 
 namespace arch {
     [[gnu::weak]] void disable_interrupts() {
@@ -72,6 +73,7 @@ namespace arch {
             x86_64::init_cpu_data();
             drivers::hpet::init();
             drivers::tsc::init();
+            drivers::pvclock::bsp_init();
             x86_64::gdt::init();
             x86_64::idt::init();
             x86_64::lapic::init(1500);
@@ -84,6 +86,7 @@ namespace arch {
             enable_paging(gobject::kernel_root);
             x86_64::init_cpu_data();
             drivers::tsc::init();
+            drivers::pvclock::init();
             x86_64::gdt::init();
             x86_64::idt::init();
             x86_64::lapic::init(1500);
@@ -108,4 +111,14 @@ namespace arch {
         return x86_64::irq::create(irq,type,func,arg,flags);
     }
 
+    [[gnu::weak]] bool test_interrupts() {
+        uint64_t rflags;
+        __asm__ __volatile__ (
+            "pushfq\n\t"
+            "pop %0" : "=rm" (rflags): : "memory"
+        );
+        return (rflags & (1 << 9)) != 0;
+    }
+
+
 }
\ No newline at end of file