From 6a12fb20ab0a8df2c4d4279b3eee59c69b5a7005 Mon Sep 17 00:00:00 2001
From: Remy Luisant <remy@luisant.ca>
Date: Fri, 19 Nov 2021 11:54:50 -0800
Subject: [PATCH] timer: Add tickless support for the MIPS CP0 timer

This commit adds support for tickless operation on the MIPS CP0 timer.
The code closely follows the Xtensa and RISCV timer drivers.

All tests pass.

Signed-off-by: Remy Luisant <remy@luisant.ca>
Signed-off-by: Antony Pavlov <antonynpavlov@gmail.com>
---
 drivers/timer/Kconfig.mips_cp0 |  1 +
 drivers/timer/mips_cp0_timer.c | 66 +++++++++++++++++++++++++++++-----
 2 files changed, 59 insertions(+), 8 deletions(-)

diff --git a/drivers/timer/Kconfig.mips_cp0 b/drivers/timer/Kconfig.mips_cp0
index ed29331a654..88b048b85c3 100644
--- a/drivers/timer/Kconfig.mips_cp0
+++ b/drivers/timer/Kconfig.mips_cp0
@@ -5,5 +5,6 @@
 config MIPS_CP0_TIMER
 	bool "MIPS CP0 Timer"
 	depends on MIPS
+	select TICKLESS_CAPABLE
 	help
 	  This module implements a kernel device driver for the MIPS CP0 timer.
diff --git a/drivers/timer/mips_cp0_timer.c b/drivers/timer/mips_cp0_timer.c
index 1e890610772..ff24809308c 100644
--- a/drivers/timer/mips_cp0_timer.c
+++ b/drivers/timer/mips_cp0_timer.c
@@ -1,7 +1,8 @@
 /*
  * Copyright (c) 2020, 2021 Antony Pavlov <antonynpavlov@gmail.com>
+ * Copyright (c) 2021 Remy Luisant <remy@luisant.ca>
  *
- * based on riscv_machine_timer.c
+ * Based on riscv_machine_timer.c and xtensa_sys_timer.c
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -19,6 +20,8 @@
 #define MAX_TICKS ((MAX_CYC - CYC_PER_TICK) / CYC_PER_TICK)
 #define MIN_DELAY 1000
 
+#define TICKLESS IS_ENABLED(CONFIG_TICKLESS_KERNEL)
+
 static struct k_spinlock lock;
 static uint32_t last_count;
 
@@ -42,21 +45,63 @@ static void timer_isr(const void *arg)
 
 	last_count = now;
 
-	uint32_t next = last_count + CYC_PER_TICK;
+	if (!TICKLESS) {
+		uint32_t next = last_count + CYC_PER_TICK;
 
-	if (next - now < MIN_DELAY) {
-		next += CYC_PER_TICK;
+		if (next - now < MIN_DELAY) {
+			next += CYC_PER_TICK;
+		}
+		set_cp0_compare(next);
 	}
-	set_cp0_compare(next);
 
 	k_spin_unlock(&lock, key);
-	sys_clock_announce(1);
+	sys_clock_announce(TICKLESS ? dticks : 1);
+}
+
+void sys_clock_set_timeout(int32_t ticks, bool idle)
+{
+	ARG_UNUSED(idle);
+
+	if (!TICKLESS) {
+		return;
+	}
+
+	ticks = ticks == K_TICKS_FOREVER ? MAX_TICKS : ticks;
+	ticks = CLAMP(ticks - 1, 0, (int32_t)MAX_TICKS);
+
+	k_spinlock_key_t key = k_spin_lock(&lock);
+	uint32_t current_count = get_cp0_count();
+	uint32_t delay_wanted = ticks * CYC_PER_TICK;
+
+	/* Round up to next tick boundary. */
+	uint32_t adj = (current_count - last_count) + (CYC_PER_TICK - 1);
+
+	if (delay_wanted <= MAX_CYC - adj) {
+		delay_wanted += adj;
+	} else {
+		delay_wanted = MAX_CYC;
+	}
+	delay_wanted = (delay_wanted / CYC_PER_TICK) * CYC_PER_TICK;
+
+	if ((int32_t)(delay_wanted + last_count - current_count) < MIN_DELAY) {
+		delay_wanted += CYC_PER_TICK;
+	}
+
+	set_cp0_compare(delay_wanted + last_count);
+	k_spin_unlock(&lock, key);
 }
 
-/* tickless kernel is not supported */
 uint32_t sys_clock_elapsed(void)
 {
-	return 0;
+	if (!TICKLESS) {
+		return 0;
+	}
+
+	k_spinlock_key_t key = k_spin_lock(&lock);
+	uint32_t ticks_elapsed = (get_cp0_count() - last_count) / CYC_PER_TICK;
+
+	k_spin_unlock(&lock, key);
+	return ticks_elapsed;
 }
 
 uint32_t sys_clock_cycle_get_32(void)
@@ -70,6 +115,11 @@ static int sys_clock_driver_init(const struct device *dev)
 
 	IRQ_CONNECT(MIPS_MACHINE_TIMER_IRQ, 0, timer_isr, NULL, 0);
 	last_count = get_cp0_count();
+
+	/*
+	 * In a tickless system the first tick might possibly be pushed
+	 * much further into the future than is being done here.
+	 */
 	set_cp0_compare(last_count + CYC_PER_TICK);
 
 	irq_enable(MIPS_MACHINE_TIMER_IRQ);