RISC-V: Add Sstc extension support

[platform/kernel/linux-rpi.git] / drivers / clocksource / timer-riscv.c

diff --git a/drivers/clocksource/timer-riscv.c b/drivers/clocksource/timer-riscv.c
index e460df7..969a552 100644 (file)
--- a/drivers/clocksource/timer-riscv.c
+++ b/drivers/clocksource/timer-riscv.c
@@ -7,6 +7,9 @@
  * either be read from the "time" and "timeh" CSRs, and can use the SBI to
  * setup events, or directly accessed using MMIO registers.
  */
+
+#define pr_fmt(fmt) "riscv-timer: " fmt
+
 #include <linux/clocksource.h>
 #include <linux/clockchips.h>
 #include <linux/cpu.h>
@@ -20,14 +23,28 @@
 #include <linux/of_irq.h>
 #include <clocksource/timer-riscv.h>
 #include <asm/smp.h>
+#include <asm/hwcap.h>
 #include <asm/sbi.h>
 #include <asm/timex.h>
 
+static DEFINE_STATIC_KEY_FALSE(riscv_sstc_available);
+
 static int riscv_clock_next_event(unsigned long delta,
                struct clock_event_device *ce)
 {
+       u64 next_tval = get_cycles64() + delta;
+
        csr_set(CSR_IE, IE_TIE);
-       sbi_set_timer(get_cycles64() + delta);
+       if (static_branch_likely(&riscv_sstc_available)) {
+#if defined(CONFIG_32BIT)
+               csr_write(CSR_STIMECMP, next_tval & 0xFFFFFFFF);
+               csr_write(CSR_STIMECMPH, next_tval >> 32);
+#else
+               csr_write(CSR_STIMECMP, next_tval);
+#endif
+       } else
+               sbi_set_timer(next_tval);
+
        return 0;
 }
 
@@ -166,6 +183,12 @@ static int __init riscv_timer_init_dt(struct device_node *n)
        if (error)
                pr_err("cpu hp setup state failed for RISCV timer [%d]\n",
                       error);
+
+       if (riscv_isa_extension_available(NULL, SSTC)) {
+               pr_info("Timer interrupt in S-mode is available via sstc extension\n");
+               static_branch_enable(&riscv_sstc_available);
+       }
+
        return error;
 }