[PATCH] riscv: Access time mmio instead of rdtime.

[Aleksandar Rikalo <arikalo@gmail.com>]

Allows for riscv-clock to pick up mmio address for faster rdtime access
via clock-reg (u64) property on boot cpu node.

Add RISCV_TIME_MMIO config.

Signed-off-by: Dragan Mladjenovic <Dragan.Mladjenovic@syrmia.com>
Signed-off-by: Aleksandar Rikalo <arikalo@gmail.com>
Signed-off-by: Djordje Todorovic <djordje.todorovic@htecgroup.com>
---
 arch/riscv/Kconfig                | 11 ++++++
 arch/riscv/include/asm/timex.h    | 59 ++++++++++++++++++++-----------
 drivers/clocksource/timer-riscv.c | 22 ++++++++++++
 3 files changed, 72 insertions(+), 20 deletions(-)

diff --git a/arch/riscv/Kconfig b/arch/riscv/Kconfig
index d4a7ca0388c0..0f4243e4bc1b 100644
--- a/arch/riscv/Kconfig
+++ b/arch/riscv/Kconfig
@@ -716,6 +716,17 @@ config RISCV_ISA_ZACAS
 
 	  If you don't know what to do here, say Y.
 
+config RISCV_TIME_MMIO
+	bool "Time mmio support"
+	depends on !RISCV_M_MODE
+	help
+	   Access time mmio instead of rdtime.
+	   Allows for riscv-clock to pick up mmio address for faster rdtime
+	   access via clock-reg (u64) property on boot cpu node.
+	   Some CPUs trap on rdtime or reading the time CSR register.
+
+	   If you don't know what to do here, say N.
+
 config TOOLCHAIN_HAS_ZBB
 	bool
 	default y
diff --git a/arch/riscv/include/asm/timex.h b/arch/riscv/include/asm/timex.h
index a06697846e69..b1956c737513 100644
--- a/arch/riscv/include/asm/timex.h
+++ b/arch/riscv/include/asm/timex.h
@@ -7,31 +7,24 @@
 #define _ASM_RISCV_TIMEX_H
 
 #include <asm/csr.h>
+#include <asm/mmio.h>
+
+#include <linux/jump_label.h>
 
 typedef unsigned long cycles_t;
 
+extern u64 __iomem *riscv_time_val;
+DECLARE_STATIC_KEY_FALSE(riscv_time_mmio_available);
+
+#define riscv_time_val riscv_time_val
+
 #ifdef CONFIG_RISCV_M_MODE
 
 #include <asm/clint.h>
 
-#ifdef CONFIG_64BIT
-static inline cycles_t get_cycles(void)
-{
-	return readq_relaxed(clint_time_val);
-}
-#else /* !CONFIG_64BIT */
-static inline u32 get_cycles(void)
-{
-	return readl_relaxed(((u32 *)clint_time_val));
-}
-#define get_cycles get_cycles
+#undef riscv_time_val
 
-static inline u32 get_cycles_hi(void)
-{
-	return readl_relaxed(((u32 *)clint_time_val) + 1);
-}
-#define get_cycles_hi get_cycles_hi
-#endif /* CONFIG_64BIT */
+#define riscv_time_val clint_time_val
 
 /*
  * Much like MIPS, we may not have a viable counter to use at an early point
@@ -46,22 +39,48 @@ static inline unsigned long random_get_entropy(void)
 }
 #define random_get_entropy()	random_get_entropy()
 
-#else /* CONFIG_RISCV_M_MODE */
+#endif
+
+static inline long use_riscv_time_mmio(void)
+{
+	return IS_ENABLED(CONFIG_RISCV_M_MODE) ||
+		(IS_ENABLED(CONFIG_RISCV_TIME_MMIO) &&
+		 static_branch_unlikely(&riscv_time_mmio_available));
+}
+
+#ifdef CONFIG_64BIT
+static inline cycles_t mmio_get_cycles(void)
+{
+	return readq_relaxed(riscv_time_val);
+}
+#else /* !CONFIG_64BIT */
+static inline cycles_t mmio_get_cycles(void)
+{
+	return readl_relaxed(((u32 *)riscv_time_val));
+}
+#endif /* CONFIG_64BIT */
+
+static inline u32 mmio_get_cycles_hi(void)
+{
+	return readl_relaxed(((u32 *)riscv_time_val) + 1);
+}
 
 static inline cycles_t get_cycles(void)
 {
+	if (use_riscv_time_mmio())
+		return mmio_get_cycles();
 	return csr_read(CSR_TIME);
 }
 #define get_cycles get_cycles
 
 static inline u32 get_cycles_hi(void)
 {
+	if (use_riscv_time_mmio())
+		return mmio_get_cycles_hi();
 	return csr_read(CSR_TIMEH);
 }
 #define get_cycles_hi get_cycles_hi
 
-#endif /* !CONFIG_RISCV_M_MODE */
-
 #ifdef CONFIG_64BIT
 static inline u64 get_cycles64(void)
 {
diff --git a/drivers/clocksource/timer-riscv.c b/drivers/clocksource/timer-riscv.c
index 48ce50c5f5e6..1f6be676d6c7 100644
--- a/drivers/clocksource/timer-riscv.c
+++ b/drivers/clocksource/timer-riscv.c
@@ -32,6 +32,13 @@
 static DEFINE_STATIC_KEY_FALSE(riscv_sstc_available);
 static bool riscv_timer_cannot_wake_cpu;
 
+#if defined(CONFIG_RISCV_TIME_MMIO)
+EXPORT_SYMBOL(riscv_time_mmio_available);
+DEFINE_STATIC_KEY_FALSE_RO(riscv_time_mmio_available);
+u64 __iomem *riscv_time_val __ro_after_init;
+EXPORT_SYMBOL(riscv_time_val);
+#endif
+
 static void riscv_clock_event_stop(void)
 {
 	if (static_branch_likely(&riscv_sstc_available)) {
@@ -203,6 +210,9 @@ static int __init riscv_timer_init_dt(struct device_node *n)
 	int cpuid, error;
 	unsigned long hartid;
 	struct device_node *child;
+#if defined(CONFIG_RISCV_TIME_MMIO)
+	u64 mmio_addr;
+#endif
 
 	error = riscv_of_processor_hartid(n, &hartid);
 	if (error < 0) {
@@ -220,6 +230,18 @@ static int __init riscv_timer_init_dt(struct device_node *n)
 	if (cpuid != smp_processor_id())
 		return 0;
 
+#if defined(CONFIG_RISCV_TIME_MMIO)
+	if (!of_property_read_u64(n, "clock-reg", &mmio_addr)) {
+		riscv_time_val = ioremap((long)mmio_addr, 8);
+		if (riscv_time_val) {
+			pr_info("Using mmio time register at 0x%llx\n", mmio_addr);
+			static_branch_enable(&riscv_time_mmio_available);
+		} else {
+			pr_warn("Unable to use mmio time at 0x%llx\n", mmio_addr);
+		}
+	}
+#endif
+
 	child = of_find_compatible_node(NULL, NULL, "riscv,timer");
 	if (child) {
 		riscv_timer_cannot_wake_cpu = of_property_read_bool(child,
-- 
2.25.1