Kernel: Make TSC based timer monotonic

Prior to this I was just blindly rebasing the current TSC stats every
once in a while. Now I check for drift and scale the multiplier
accordingly to keep the timer from drifting

kernel/include/kernel/API/SharedPage.h

@@ -18,9 +18,8 @@ namespace Kernel::API
 
 		struct
 		{
-			uint8_t shift;
+			uint64_t realtime_s;
-			uint64_t mult;
+			uint32_t realtime_ns;
-			uint64_t realtime_seconds;
 		} gettime_shared;
 
 		struct
@@ -28,6 +27,8 @@ namespace Kernel::API
 			struct
 			{
 				uint32_t seq;
+				uint32_t mult;
+				int8_t shift;
 				uint64_t last_ns;
 				uint64_t last_tsc;
 			} gettime_local;

kernel/include/kernel/CPUID.h

@@ -82,5 +82,6 @@ namespace CPUID
 	bool has_pat();
 	bool has_1gib_pages();
 	bool has_invariant_tsc();
+	uint64_t get_tsc_frequency();
 
 }

kernel/include/kernel/Processor.h

@@ -124,7 +124,7 @@ namespace Kernel
 		static void yield();
 		static Scheduler& scheduler() { return *read_gs_sized<Scheduler*>(offsetof(Processor, m_scheduler)); }
 
-		static void initialize_tsc(uint8_t shift, uint64_t mult, uint64_t realtime_seconds);
+		static void initialize_tsc(uint64_t realtime_seconds);
 		static void update_tsc();
 		static uint64_t ns_since_boot_tsc();
 

kernel/include/kernel/Timer/Timer.h

@@ -30,6 +30,13 @@ namespace Kernel
 
 	class SystemTimer : public Timer
 	{
+	public:
+		struct TSCInfo
+		{
+			int8_t shift;
+			uint32_t mult;
+		};
+
 	public:
 		static void initialize();
 		static SystemTimer& get();
@@ -49,8 +56,10 @@ namespace Kernel
 
 		void dont_invoke_scheduler() { m_timer->m_should_invoke_scheduler = false; }
 
-		void update_tsc() const;
+		void update_tsc();
-		uint64_t ns_since_boot_no_tsc() const;
+		TSCInfo tsc_info() const;
+
+		uint64_t ns_since_boot_no_tsc() const { return m_timer->ns_since_boot(); }
 
 		timespec real_time() const;
 
@@ -59,14 +68,29 @@ namespace Kernel
 
 		void initialize_timers();
 
-		uint64_t get_tsc_frequency() const;
+		void initialize_invariant_tsc();
 
 	private:
+		enum class TSCType
+		{
+			None,
+			Invariant,
+		};
+
 		uint64_t m_boot_time { 0 };
 		BAN::UniqPtr<RTC> m_rtc;
 		BAN::UniqPtr<Timer> m_timer;
-		bool m_has_invariant_tsc { false };
+
-		mutable uint32_t m_timer_ticks { 0 };
+		TSCType m_tsc_type = TSCType::None;
+
+		union {
+			struct {
+				int8_t shift;
+				uint32_t mult;
+			} invariant;
+		} m_tsc_info;
+
+		uint64_t m_tsc_update_ns { 0 };
 	};
 
 }

kernel/kernel/CPUID.cpp

@@ -85,6 +85,18 @@ namespace CPUID
 		return buffer[3] & (1 << 8);
 	}
 
+	uint64_t get_tsc_frequency()
+	{
+		uint32_t buffer[4];
+		get_cpuid(0x00, buffer);
+		if (buffer[0] < 0x15)
+			return 0;
+		get_cpuid(0x15, buffer);
+		if (buffer[0] == 0 || buffer[1] == 0 || buffer[2] == 0)
+			return 0;
+		return static_cast<uint64_t>(buffer[2]) * buffer[1] / buffer[0];
+	}
+
 	const char* feature_string_ecx(uint32_t feat)
 	{
 		switch (feat)

kernel/kernel/Processor.cpp

@@ -289,13 +289,11 @@ namespace Kernel
 		}
 	}
 
-	void Processor::initialize_tsc(uint8_t shift, uint64_t mult, uint64_t realtime_seconds)
+	void Processor::initialize_tsc(uint64_t realtime_seconds)
 	{
 		auto& shared_page = Processor::shared_page();
-
+		shared_page.gettime_shared.realtime_s = realtime_seconds;
-		shared_page.gettime_shared.shift = shift;
+		shared_page.gettime_shared.realtime_ns = 0;
-		shared_page.gettime_shared.mult = mult;
-		shared_page.gettime_shared.realtime_seconds = realtime_seconds;
 
 		update_tsc();
 
@@ -322,23 +320,57 @@ namespace Kernel
 
 	void Processor::update_tsc()
 	{
-		auto& sgettime = shared_page().cpus[current_index()].gettime_local;
+		auto& lgettime = shared_page().cpus[current_index()].gettime_local;
-		sgettime.seq = sgettime.seq + 1;
+		lgettime.seq = lgettime.seq + 1;
-		sgettime.last_ns = SystemTimer::get().ns_since_boot_no_tsc();
+
-		sgettime.last_tsc = __builtin_ia32_rdtsc();
+		if (lgettime.seq == 1)
-		sgettime.seq = sgettime.seq + 1;
+		{
+			const auto tsc_info = SystemTimer::get().tsc_info();
+			lgettime.shift = tsc_info.shift;
+			lgettime.mult  = tsc_info.mult;
+			lgettime.last_ns  = SystemTimer::get().ns_since_boot_no_tsc();
+			lgettime.last_tsc = __builtin_ia32_rdtsc();
+		}
+		else
+		{
+			const auto current_ns = SystemTimer::get().ns_since_boot_no_tsc();
+			const auto current_tsc = __builtin_ia32_rdtsc();
+
+			auto delta_ns = current_tsc - lgettime.last_tsc;
+			if (lgettime.shift >= 0)
+				delta_ns <<= lgettime.shift;
+			else
+				delta_ns >>= -lgettime.shift;
+			delta_ns = (delta_ns * lgettime.mult) >> 32;
+
+			lgettime.last_ns += delta_ns;
+			lgettime.last_tsc = current_tsc;
+
+			// scale mult by [-0.25%, 0.25%] to fix for clock drift
+			const auto error_ns = static_cast<int64_t>(current_ns) - static_cast<int64_t>(lgettime.last_ns);
+			const auto correction_ppm = BAN::Math::clamp<int64_t>(error_ns * 1'000'000 / 1'000'000'000, -100, 100);
+			const auto correction_delta = -lgettime.mult * correction_ppm / 1'000'000;
+			lgettime.mult += correction_delta;
+		}
+
+		lgettime.seq = lgettime.seq + 1;
 	}
 
 	uint64_t Processor::ns_since_boot_tsc()
 	{
 		const auto& shared_page = Processor::shared_page();
-		const auto& sgettime = shared_page.gettime_shared;
 		const auto& lgettime = shared_page.cpus[current_index()].gettime_local;
 
 		auto state = get_interrupt_state();
 		set_interrupt_state(InterruptState::Disabled);
 
-		const auto current_ns = lgettime.last_ns + (((__builtin_ia32_rdtsc() - lgettime.last_tsc) * sgettime.mult) >> sgettime.shift);
+		uint64_t current_ns = __builtin_ia32_rdtsc() - lgettime.last_tsc;
+		if (lgettime.shift >= 0)
+			current_ns <<= lgettime.shift;
+		else
+			current_ns >>= -lgettime.shift;
+		current_ns = (current_ns * lgettime.mult) >> 32;
+		current_ns += lgettime.last_ns;
 
 		set_interrupt_state(state);
 

kernel/kernel/Timer/Timer.cpp

@@ -59,62 +59,65 @@ namespace Kernel
 
 	void SystemTimer::initialize_tsc()
 	{
-		if (!CPUID::has_invariant_tsc())
+		if (CPUID::has_invariant_tsc())
-		{
+			return initialize_invariant_tsc();
-			dwarnln("CPU does not have an invariant TSC");
+		dwarnln("No supported TSC based timers available");
-			return;
+	}
-		}
 
-		const uint64_t tsc_freq = get_tsc_frequency();
+	void SystemTimer::initialize_invariant_tsc()
+	{
+		const uint64_t tsc_freq = [this]() -> uint64_t {
+			if (const auto cpuid_freq = CPUID::get_tsc_frequency())
+				return cpuid_freq;
+
+			// take 5x 50 ms samples and use the median value
+
+			constexpr size_t tsc_sample_count = 5;
+			constexpr size_t tsc_sample_ns = 50'000'000;
+
+			uint64_t tsc_freq_samples[tsc_sample_count];
+			for (size_t i = 0; i < tsc_sample_count; i++)
+			{
+				const auto start_ns = m_timer->ns_since_boot();
+
+				const auto start_tsc = __builtin_ia32_rdtsc();
+				while (m_timer->ns_since_boot() < start_ns + tsc_sample_ns)
+					Processor::pause();
+				const auto stop_tsc = __builtin_ia32_rdtsc();
+
+				const auto stop_ns = m_timer->ns_since_boot();
+
+				const auto duration_ns = stop_ns - start_ns;
+				const auto count_tsc = stop_tsc - start_tsc;
+
+				tsc_freq_samples[i] = count_tsc * 1'000'000'000 / duration_ns;
+			}
+
+			BAN::sort::sort(tsc_freq_samples, tsc_freq_samples + tsc_sample_count);
+
+			return tsc_freq_samples[tsc_sample_count / 2];
+		}();
+
+		m_tsc_info = { .invariant = {
+			.shift = 0,
+			.mult  = static_cast<uint32_t>((1'000'000'000ull << 32) / tsc_freq),
+		}};
+		m_tsc_type = TSCType::Invariant;
+		Processor::initialize_tsc(m_boot_time);
 
 		dprintln("Initialized invariant TSC ({} Hz)", tsc_freq);
-
-		const uint8_t tsc_shift = 22;
-		const uint64_t tsc_mult = (static_cast<uint64_t>(1'000'000'000) << tsc_shift) / tsc_freq;
-		Processor::initialize_tsc(tsc_shift, tsc_mult, m_boot_time);
-
-		m_has_invariant_tsc = true;
 	}
 
-	uint64_t SystemTimer::get_tsc_frequency() const
+	void SystemTimer::update_tsc()
 	{
-		// take 5x 50 ms samples and use the median value
+		if (m_tsc_type == TSCType::None)
-
-		constexpr size_t tsc_sample_count = 5;
-		constexpr size_t tsc_sample_ns = 50'000'000;
-
-		uint64_t tsc_freq_samples[tsc_sample_count];
-		for (size_t i = 0; i < tsc_sample_count; i++)
-		{
-			const auto start_ns = m_timer->ns_since_boot();
-
-			const auto start_tsc = __builtin_ia32_rdtsc();
-			while (m_timer->ns_since_boot() < start_ns + tsc_sample_ns)
-				Processor::pause();
-			const auto stop_tsc = __builtin_ia32_rdtsc();
-
-			const auto stop_ns = m_timer->ns_since_boot();
-
-			const auto duration_ns = stop_ns - start_ns;
-			const auto count_tsc = stop_tsc - start_tsc;
-
-			tsc_freq_samples[i] = count_tsc * 1'000'000'000 / duration_ns;
-		}
-
-		BAN::sort::sort(tsc_freq_samples, tsc_freq_samples + tsc_sample_count);
-
-		return tsc_freq_samples[tsc_sample_count / 2];
-	}
-
-	void SystemTimer::update_tsc() const
-	{
-		if (!m_has_invariant_tsc)
 			return;
 
-		// only update every 100 ms
+		// only update once per second
-		if (++m_timer_ticks < 100)
+		const uint64_t current_ns = Processor::ns_since_boot_tsc();
+		if (current_ns < m_tsc_update_ns)
 			return;
-		m_timer_ticks = 0;
+		m_tsc_update_ns = current_ns + 1'000'000'000;
 
 		Processor::update_tsc();
 		Processor::broadcast_smp_message({
@@ -123,28 +126,38 @@ namespace Kernel
 		});
 	}
 
-	uint64_t SystemTimer::ns_since_boot_no_tsc() const
+	SystemTimer::TSCInfo SystemTimer::tsc_info() const
 	{
-		return m_timer->ns_since_boot();
+		switch (m_tsc_type)
+		{
+			case TSCType::None:
+				ASSERT_NOT_REACHED();
+			case TSCType::Invariant:
+				return {
+					.shift = m_tsc_info.invariant.shift,
+					.mult  = m_tsc_info.invariant.mult,
+				};
+		}
+		ASSERT_NOT_REACHED();
 	}
 
 	uint64_t SystemTimer::ms_since_boot() const
 	{
-		if (!m_has_invariant_tsc)
+		if (m_tsc_type == TSCType::None)
 			return m_timer->ms_since_boot();
 		return Processor::ns_since_boot_tsc() / 1'000'000;
 	}
 
 	uint64_t SystemTimer::ns_since_boot() const
 	{
-		if (!m_has_invariant_tsc)
+		if (m_tsc_type == TSCType::None)
 			return m_timer->ns_since_boot();
 		return Processor::ns_since_boot_tsc();
 	}
 
 	timespec SystemTimer::time_since_boot() const
 	{
-		if (!m_has_invariant_tsc)
+		if (m_tsc_type == TSCType::None)
 			return m_timer->time_since_boot();
 		const auto ns_since_boot = Processor::ns_since_boot_tsc();
 		return {

userspace/libraries/LibC/time.cpp

@@ -46,7 +46,13 @@ int clock_gettime(clockid_t clock_id, struct timespec* tp)
 		if (old_seq & 1)
 			continue;
 
-		const auto monotonic_ns = lgettime.last_ns + (((__builtin_ia32_rdtsc() - lgettime.last_tsc) * sgettime.mult) >> sgettime.shift);
+		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;
@@ -57,7 +63,15 @@ int clock_gettime(clockid_t clock_id, struct timespec* tp)
 		};
 
 		if (clock_id == CLOCK_REALTIME)
-			tp->tv_sec += sgettime.realtime_seconds;
+		{
+			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;
 	}