From 47360a9861ed37d8085bf583df622f27b69410f3 Mon Sep 17 00:00:00 2001
From: David Chisnall <github@theravensnest.org>
Date: Fri, 12 Apr 2024 15:43:12 +0100
Subject: [PATCH] Tickless scheduler step 3: Correct for tick lengths.

Collect the error in the tick rate and correct the current tick to be a
close approximation of the value that we'd have with a ticky scheduler.
---
 sdk/core/scheduler/main.cc  |  1 +
 sdk/core/scheduler/thread.h | 19 +++++++++++++++++++
 sdk/core/scheduler/timer.h  | 37 +++++++++++++++++++++++++++++++------
 3 files changed, 51 insertions(+), 6 deletions(-)

diff --git a/sdk/core/scheduler/main.cc b/sdk/core/scheduler/main.cc
index 46197310..071d224f 100644
--- a/sdk/core/scheduler/main.cc
+++ b/sdk/core/scheduler/main.cc
@@ -298,6 +298,7 @@ namespace sched
 					  std::tie(schedNeeded, std::ignore, std::ignore) =
 					    futex_wake(Capability{&word}.address());
 				  });
+				tick = schedNeeded;
 				break;
 			case MCAUSE_THREAD_EXIT:
 				// Make the current thread non-runnable.
diff --git a/sdk/core/scheduler/thread.h b/sdk/core/scheduler/thread.h
index 99cb269a..009472e5 100644
--- a/sdk/core/scheduler/thread.h
+++ b/sdk/core/scheduler/thread.h
@@ -233,6 +233,12 @@ namespace
 					schedule        = true;
 				}
 			}
+			// If this is the same priority as the current thread, we may need
+			// to update the timer.
+			if (priority >= highestPriority)
+			{
+				schedule        = true;
+			}
 			if (reason == WakeReason::Timer || reason == WakeReason::Delete)
 			{
 				multiWaiter = nullptr;
@@ -516,6 +522,19 @@ namespace
 			return state == ThreadState::Ready;
 		}
 
+		/**
+		 * Returns true if there are other runnable threads with the same
+		 * priority as this thread.
+		 */
+		bool has_priority_peers()
+		{
+			Debug::Assert(state == ThreadState::Ready,
+			              "Checking for peers on thread that is in state {}, "
+			              "not ready",
+			              state);
+			return next != this;
+		}
+
 		~ThreadImpl()
 		{
 			// We have static definition of threads. We only create threads in
diff --git a/sdk/core/scheduler/timer.h b/sdk/core/scheduler/timer.h
index 283844a2..4ed3b0de 100644
--- a/sdk/core/scheduler/timer.h
+++ b/sdk/core/scheduler/timer.h
@@ -28,7 +28,9 @@ namespace
 
 	class Timer final : private TimerCore
 	{
-		inline static uint64_t lastTickTime = 0;
+		inline static uint64_t lastTickTime         = 0;
+		inline static uint32_t accumulatedTickError = 0;
+
 		public:
 		static void interrupt_setup()
 		{
@@ -41,14 +43,23 @@ namespace
 
 		static void update()
 		{
-
-			if (Thread::waitingList == nullptr)
+			auto *thread             = Thread::current_get();
+			bool  waitingListIsEmpty = ((Thread::waitingList == nullptr) ||
+                                       (Thread::waitingList->expiryTime == -1));
+			bool  threadHasNoPeers =
+			  (thread == nullptr) || (!thread->has_priority_peers());
+			if (waitingListIsEmpty && threadHasNoPeers)
 			{
+				Debug::log("No threads waiting on timer");
 				clear();
 			}
 			else
 			{
-				setnext(TIMERCYCLES_PER_TICK * (Thread::waitingList->expiryTime - Thread::ticksSinceBoot));
+				uint64_t ticksToWait = waitingListIsEmpty
+				                         ? 1
+				                         : (Thread::waitingList->expiryTime -
+				                            Thread::ticksSinceBoot);
+				setnext(TIMERCYCLES_PER_TICK * ticksToWait);
 			}
 		}
 
@@ -56,10 +67,24 @@ namespace
 		{
 			// TODO: Should be reading the timer's time, not the core's time.
 			// They are currently the same value, but that's not guaranteed.
-			uint64_t now = rdcycle64();
+			uint64_t now     = rdcycle64();
 			uint32_t elapsed = now - lastTickTime;
+			int32_t  error   = elapsed % TIMERCYCLES_PER_TICK;
+			if (elapsed < TIMERCYCLES_PER_TICK)
+			{
+				error = TIMERCYCLES_PER_TICK - error;
+			}
+			accumulatedTickError += error;
+			int32_t errorDirection = accumulatedTickError < 0 ? -1 : 1;
+			int32_t absoluteError  = accumulatedTickError * errorDirection;
+			if (absoluteError >= TIMERCYCLES_PER_TICK)
+			{
+				Thread::ticksSinceBoot += errorDirection;
+				accumulatedTickError += TIMERCYCLES_PER_TICK * -errorDirection;
+			}
 			lastTickTime = now;
-			Thread::ticksSinceBoot += std::max(1U, elapsed / TIMERCYCLES_PER_TICK);
+			Thread::ticksSinceBoot +=
+			  std::max(1U, elapsed / TIMERCYCLES_PER_TICK);
 			if (Thread::waitingList == nullptr)
 			{
 				return;