Kernel/LibC: support RDTSCP based clock_gettime

RDTSCP seems to be faster than LSL and it removes the need for getting
the current cpu twice to make sure TSC is read on the correct CPU

kernel/include/kernel/API/SharedPage.h

@@ -8,6 +8,7 @@ namespace Kernel::API
 	enum SharedPageFeature : uint32_t
 	{
 		SPF_GETTIME = 1 << 0,
+		SPF_RDTSCP  = 1 << 1,
 	};
 
 	struct SharedPage

kernel/include/kernel/CPUID.h

@@ -82,6 +82,7 @@ namespace CPUID
 	bool has_pat();
 	bool has_1gib_pages();
 	bool has_invariant_tsc();
+	bool has_rdtscp();
 	uint64_t get_tsc_frequency();
 	bool has_kvm_pvclock();
 

kernel/kernel/CPUID.cpp

@@ -85,6 +85,16 @@ namespace CPUID
 		return buffer[3] & (1 << 8);
 	}
 
+	bool has_rdtscp()
+	{
+		uint32_t buffer[4] {};
+		get_cpuid(0x80000000, buffer);
+		if (buffer[0] < 0x80000001)
+			return false;
+		get_cpuid(0x80000001, buffer);
+		return buffer[3] & (1 << 27);
+	}
+
 	uint64_t get_tsc_frequency()
 	{
 		uint32_t buffer[4];

kernel/kernel/Processor.cpp

@@ -1,3 +1,4 @@
+#include <kernel/CPUID.h>
 #include <kernel/InterruptController.h>
 #include <kernel/Memory/Heap.h>
 #include <kernel/Memory/kmalloc.h>
@@ -20,6 +21,8 @@ namespace Kernel
 	static constexpr uint32_t MSR_IA32_FMASK = 0xC0000084;
 #endif
 
+	static constexpr uint32_t MSR_IA32_TSC_AUX = 0xC0000103;
+
 	ProcessorID          Processor::s_bsp_id                     { PROCESSOR_NONE };
 	BAN::Atomic<uint8_t> Processor::s_processor_count            { 0 };
 	BAN::Atomic<bool>    Processor::s_is_smp_enabled             { false };
@@ -219,6 +222,9 @@ namespace Kernel
 		shared_page.gdt_cpu_offset = GDT::cpu_index_offset();
 		shared_page.features = 0;
 
+		if (CPUID::has_rdtscp())
+			shared_page.features |= API::SPF_RDTSCP;
+
 		ASSERT(Processor::count() + sizeof(Kernel::API::SharedPage) <= PAGE_SIZE);
 	}
 
@@ -321,7 +327,10 @@ namespace Kernel
 	void Processor::update_tsc()
 	{
 		auto& lgettime = shared_page().cpus[current_index()].gettime_local;
-		lgettime.seq = lgettime.seq + 1;
+
+		const auto seq = BAN::atomic_load(lgettime.seq, BAN::memory_order_relaxed);
+
+		BAN::atomic_store(lgettime.seq, seq + 1, BAN::memory_order_release);
 
 		if (lgettime.seq == 1)
 		{
@@ -330,6 +339,9 @@ namespace Kernel
 			lgettime.mult  = tsc_info.mult;
 			lgettime.last_ns  = SystemTimer::get().ns_since_boot_no_tsc();
 			lgettime.last_tsc = __builtin_ia32_rdtsc();
+
+			if (CPUID::has_rdtscp())
+				asm volatile("wrmsr" :: "d"(0x00000000), "a"(current_index()), "c"(MSR_IA32_TSC_AUX));
 		}
 		else
 		{
@@ -353,7 +365,7 @@ namespace Kernel
 			lgettime.mult += correction_delta;
 		}
 
-		lgettime.seq = lgettime.seq + 1;
+		BAN::atomic_store(lgettime.seq, seq + 2, BAN::memory_order_release);
 	}
 
 	uint64_t Processor::ns_since_boot_tsc()

userspace/libraries/LibC/time.cpp

@@ -1,3 +1,4 @@
+#include <BAN/Atomic.h>
 #include <BAN/Assert.h>
 #include <BAN/Debug.h>
 #include <BAN/Math.h>
@@ -28,53 +29,68 @@ int clock_gettime(clockid_t clock_id, struct timespec* tp)
 	if (g_shared_page == nullptr || !(g_shared_page->features & Kernel::API::SPF_GETTIME))
 		return syscall(SYS_CLOCK_GETTIME, clock_id, tp);
 
-	const auto get_cpu =
+	uint32_t mult;
-		[]() -> uint8_t {
+	int8_t   shift;
-			uint16_t limit;
+	uint64_t last_ns;
-			asm volatile("lsl %1, %0" : "=r"(limit) : "r"(g_shared_page->gdt_cpu_offset));
+	uint64_t last_tsc;
-			return limit;
+	uint64_t curr_tsc;
-		};
 
-	for (;;)
+	const auto read_tsc_info = [&](volatile decltype(g_shared_page->cpus[0].gettime_local)& lgettime) {
+		uint32_t seq1, seq2;
+		do {
+			seq1     = BAN::atomic_load(lgettime.seq, BAN::memory_order_acquire);
+			mult     = lgettime.mult;
+			shift    = lgettime.shift;
+			last_ns  = lgettime.last_ns;
+			last_tsc = lgettime.last_tsc;
+			seq2     = BAN::atomic_load(lgettime.seq, BAN::memory_order_acquire);
+		} while (seq1 != seq2 || (seq1 & 1));
+	};
+
+read_tsc_info:
+	if (g_shared_page->features & Kernel::API::SPF_RDTSCP)
 	{
-		const auto cpu = get_cpu();
+		uint32_t cpu;
-
+		curr_tsc = __builtin_ia32_rdtscp(&cpu);
-		const auto& sgettime = g_shared_page->gettime_shared;
+		read_tsc_info(g_shared_page->cpus[cpu].gettime_local);
-		const auto& lgettime = g_shared_page->cpus[cpu].gettime_local;
-
-		const auto old_seq = lgettime.seq;
-		if (old_seq & 1)
-			continue;
-
-		uint64_t monotonic_ns = __builtin_ia32_rdtsc() - lgettime.last_tsc;
-		if (lgettime.shift >= 0)
-			monotonic_ns <<= lgettime.shift;
-		else
-			monotonic_ns >>= -lgettime.shift;
-		monotonic_ns = (monotonic_ns * lgettime.mult) >> 32;
-		monotonic_ns += lgettime.last_ns;
-
-		if (old_seq != lgettime.seq || cpu != get_cpu())
-			continue;
-
-		*tp = {
-			.tv_sec = static_cast<time_t>(monotonic_ns / 1'000'000'000),
-			.tv_nsec = static_cast<long>(monotonic_ns % 1'000'000'000)
-		};
-
-		if (clock_id == CLOCK_REALTIME)
-		{
-			tp->tv_sec += sgettime.realtime_s;
-			tp->tv_nsec += sgettime.realtime_ns;
-			if (tp->tv_nsec >= 1'000'000'000)
-			{
-				tp->tv_sec += tp->tv_nsec / 1'000'000'000;
-				tp->tv_nsec = tp->tv_nsec % 1'000'000'000;
-			}
-		}
-
-		return 0;
 	}
+	else for (;;)
+	{
+		uint16_t cpu1, cpu2;
+		asm volatile("lsl %1, %0" : "=r"(cpu1) : "r"(g_shared_page->gdt_cpu_offset));
+		curr_tsc = __builtin_ia32_rdtsc();
+		asm volatile("lsl %1, %0" : "=r"(cpu2) : "r"(g_shared_page->gdt_cpu_offset));
+		if (cpu1 != cpu2)
+			continue;
+		read_tsc_info(g_shared_page->cpus[cpu1].gettime_local);
+		break;
+	}
+
+	// NOTE: as we read TSC before getting the calibration, it is possible
+	//       for the calibration to get updated in between.
+	if (curr_tsc < last_tsc)
+		goto read_tsc_info;
+
+	uint64_t clock_ns = curr_tsc - last_tsc;
+	if (shift >= 0)
+		clock_ns <<= shift;
+	else
+		clock_ns >>= -shift;
+	clock_ns = (clock_ns * mult) >> 32;
+	clock_ns += last_ns;
+
+	if (clock_id == CLOCK_REALTIME)
+	{
+		const auto& sgettime = g_shared_page->gettime_shared;
+		clock_ns += sgettime.realtime_s * 1'000'000'000 + sgettime.realtime_ns;
+	}
+
+	*tp = {
+		.tv_sec = static_cast<time_t>(clock_ns / 1'000'000'000),
+		.tv_nsec = static_cast<long>(clock_ns % 1'000'000'000)
+	};
+
+	return 0;
 }
 
 int clock_getres(clockid_t clock_id, struct timespec* res)