From 4e37240cb91dd8c16c609fcd8266303421fbe6e1 Mon Sep 17 00:00:00 2001
From: Ivan Chikish <nekotekina@gmail.com>
Date: Sat, 15 Jul 2023 10:15:32 +0300
Subject: [PATCH] Implement basic TSC calibration

---
 orbis-kernel/include/orbis/KernelContext.hpp |  4 ++
 orbis-kernel/src/KernelContext.cpp           | 46 +++++++++++++++++++-
 orbis-kernel/src/sys/sys_sysctl.cpp          |  3 +-
 3 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/orbis-kernel/include/orbis/KernelContext.hpp b/orbis-kernel/include/orbis/KernelContext.hpp
index 01ce4d95..10992449 100644
--- a/orbis-kernel/include/orbis/KernelContext.hpp
+++ b/orbis-kernel/include/orbis/KernelContext.hpp
@@ -51,6 +51,8 @@ class alignas(__STDCPP_DEFAULT_NEW_ALIGNMENT__) KernelContext final {
   void deleteProcess(Process *proc);
   Process *findProcessById(pid_t pid) const;
 
+  long getTscFreq();
+
   void *kalloc(std::size_t size,
                std::size_t align = __STDCPP_DEFAULT_NEW_ALIGNMENT__);
   void kfree(void *ptr, std::size_t size);
@@ -113,6 +115,8 @@ class alignas(__STDCPP_DEFAULT_NEW_ALIGNMENT__) KernelContext final {
 
   UmtxChain m_umtx_chains[2][c_umtx_chains]{};
 
+  std::atomic<long> m_tsc_freq{0};
+
   mutable shared_mutex m_proc_mtx;
   utils::LinkedNode<Process> *m_processes = nullptr;
   utils::kmap<utils::kstring, Ref<EventFlag>> m_event_flags;
diff --git a/orbis-kernel/src/KernelContext.cpp b/orbis-kernel/src/KernelContext.cpp
index 48e056d8..7533bea7 100644
--- a/orbis-kernel/src/KernelContext.cpp
+++ b/orbis-kernel/src/KernelContext.cpp
@@ -24,7 +24,8 @@ KernelContext::KernelContext() {
   pthread_mutex_init(&m_heap_mtx, &mtx_attr);
   pthread_mutexattr_destroy(&mtx_attr);
 
-  std::printf("orbis::KernelContext initialized, addr=%p", this);
+  std::printf("orbis::KernelContext initialized, addr=%p\n", this);
+  std::printf("TSC frequency: %lu\n", getTscFreq());
 }
 KernelContext::~KernelContext() {}
 
@@ -73,6 +74,49 @@ Process *KernelContext::findProcessById(pid_t pid) const {
   return nullptr;
 }
 
+long KernelContext::getTscFreq() {
+  auto cal_tsc = []() -> long {
+    const long timer_freq = 1'000'000'000;
+
+    // Calibrate TSC
+    constexpr int samples = 40;
+    long rdtsc_data[samples];
+    long timer_data[samples];
+    long error_data[samples];
+
+    struct timespec ts0;
+    clock_gettime(CLOCK_MONOTONIC, &ts0);
+    long sec_base = ts0.tv_sec;
+
+    for (int i = 0; i < samples; i++) {
+      usleep(200);
+      error_data[i] = (__builtin_ia32_lfence(), __builtin_ia32_rdtsc());
+      struct timespec ts;
+      clock_gettime(CLOCK_MONOTONIC, &ts);
+      rdtsc_data[i] = (__builtin_ia32_lfence(), __builtin_ia32_rdtsc());
+      timer_data[i] = ts.tv_nsec + (ts.tv_sec - sec_base) * 1'000'000'000;
+    }
+
+    // Compute average TSC
+    long acc = 0;
+    for (int i = 0; i < samples - 1; i++) {
+      acc += (rdtsc_data[i + 1] - rdtsc_data[i]) * timer_freq /
+             (timer_data[i + 1] - timer_data[i]);
+    }
+
+    // Rounding
+    acc /= (samples - 1);
+    constexpr long grain = 1'000'000;
+    return grain * (acc / grain + long{(acc % grain) > (grain / 2)});
+  };
+
+  long freq = m_tsc_freq.load();
+  if (freq)
+    return freq;
+  m_tsc_freq.compare_exchange_strong(freq, cal_tsc());
+  return m_tsc_freq.load();
+}
+
 void *KernelContext::kalloc(std::size_t size, std::size_t align) {
   size = (size + (__STDCPP_DEFAULT_NEW_ALIGNMENT__ - 1)) &
          ~(__STDCPP_DEFAULT_NEW_ALIGNMENT__ - 1);
diff --git a/orbis-kernel/src/sys/sys_sysctl.cpp b/orbis-kernel/src/sys/sys_sysctl.cpp
index 1afeff41..ad0c9288 100644
--- a/orbis-kernel/src/sys/sys_sysctl.cpp
+++ b/orbis-kernel/src/sys/sys_sysctl.cpp
@@ -1,3 +1,4 @@
+#include "KernelContext.hpp"
 #include "sys/sysproto.hpp"
 
 orbis::SysResult orbis::sys___sysctl(Thread *thread, ptr<sint> name,
@@ -232,7 +233,7 @@ orbis::SysResult orbis::sys___sysctl(Thread *thread, ptr<sint> name,
           return ErrorCode::INVAL;
         }
 
-        *(uint64_t *)old = 1000000000ull;
+        *(uint64_t *)old = g_context.getTscFreq();
         return {};
 
       default: