Scheduler: Optimize by adding a proper reschedule function

include/scheduler.hpp

@@ -29,6 +29,8 @@ struct Scheduler {
 	// Set nextTimestamp to the timestamp of the next event
 	void updateNextTimestamp() { nextTimestamp = events.cbegin()->first; }
 
+	// Add an event to the scheduler. Assumes this event doesn't already exist in the scheduler.
+	// (If it might, then use rescheduleEvent instead, which will remove and reschedule the event)
 	void addEvent(EventType type, u64 timestamp) {
 		events.emplace(timestamp, type);
 		updateNextTimestamp();
@@ -46,6 +48,47 @@ struct Scheduler {
 		}
 	};
 
+	// Reschedule an event of "type" to "newTimestamp".
+	// If the event is already in the scheduler, replace its timestamp with the new timestamp
+	// If possible, it will perform an in-place replacement. Otherwise, it will fallback to a remove+insert operation.
+	// If the event is not in the scheduler, we'll add it
+	void rescheduleEvent(EventType type, u64 newTimestamp) {
+		// Find the event if it exists
+		for (auto it = events.begin(); it != events.end(); ++it) {
+			if (it->second == type) {
+				bool inplace = true;
+
+				// Peek at the previous and next events to see if we can safely update in-place
+				if (it != events.begin()) {
+					auto previousIterator = std::prev(it);
+					if (newTimestamp < previousIterator->first) {
+						inplace = false;
+					}
+				}
+
+				auto nextIterator = std::next(it);
+				if (nextIterator != events.end() && newTimestamp > nextIterator->first) {
+					inplace = false;
+				}
+
+				if (inplace) {
+					it->first = newTimestamp;
+					updateNextTimestamp();
+				} else {
+					EventType ev = it->second;
+					events.erase(it);
+					events.emplace(newTimestamp, ev);
+					updateNextTimestamp();
+				}
+
+				return;
+			}
+		}
+
+		// The event did not exist: Add it to the scheduler
+		addEvent(type, newTimestamp);
+	}
+
 	void reset() {
 		currentTimestamp = 0;
 

src/core/kernel/threads.cpp

@@ -738,8 +738,7 @@ void Kernel::addWakeupEvent(u64 tick) {
 		nextScheduledWakeupTick = tick;
 		auto& scheduler = cpu.getScheduler();
 
-		scheduler.removeEvent(Scheduler::EventType::ThreadWakeup);
-		scheduler.addEvent(Scheduler::EventType::ThreadWakeup, tick);
+		scheduler.rescheduleEvent(Scheduler::EventType::ThreadWakeup, tick);
 	}
 }
 

src/core/kernel/timers.cpp

@@ -52,9 +52,7 @@ void Kernel::pollTimers() {
 	}
 }
 
-void Kernel::cancelTimer(Timer* timer) {
-	timer->running = false;
-}
+void Kernel::cancelTimer(Timer* timer) { timer->running = false; }
 
 void Kernel::signalTimer(Handle timerHandle, Timer* timer) {
 	timer->fired = true;
@@ -115,8 +113,7 @@ void Kernel::svcSetTimer() {
 
 	Scheduler& scheduler = cpu.getScheduler();
 	// Signal an event to poll timers as soon as possible
-	scheduler.removeEvent(Scheduler::EventType::UpdateTimers);
-	scheduler.addEvent(Scheduler::EventType::UpdateTimers, cpu.getTicks() + 1);
+	scheduler.rescheduleEvent(Scheduler::EventType::UpdateTimers, cpu.getTicks() + 1);
 
 	// If the initial delay is 0 then instantly signal the timer
 	if (initial == 0) {

src/core/services/ir/circlepad_pro.cpp

@@ -19,8 +19,7 @@ void CirclePadPro::receivePayload(Payload payload) {
 			// Convert to cycles
 			period = Scheduler::nsToCycles(periodNs);
 
-			scheduler.removeEvent(Scheduler::EventType::UpdateIR);
-			scheduler.addEvent(Scheduler::EventType::UpdateIR, scheduler.currentTimestamp + period);
+			scheduler.rescheduleEvent(Scheduler::EventType::UpdateIR, scheduler.currentTimestamp + period);
 			break;
 		}
 

src/core/services/y2r.cpp

@@ -447,8 +447,7 @@ void Y2RService::startConversion(u32 messagePointer) {
 
 	// Remove any potential pending Y2R event and schedule a new one
 	Scheduler& scheduler = kernel.getScheduler();
-	scheduler.removeEvent(Scheduler::EventType::SignalY2R);
-	scheduler.addEvent(Scheduler::EventType::SignalY2R, scheduler.currentTimestamp + delayTicks);
+	scheduler.rescheduleEvent(Scheduler::EventType::SignalY2R, scheduler.currentTimestamp + delayTicks);
 }
 
 void Y2RService::isFinishedSendingYUV(u32 messagePointer) {