RISC-V: Add Sstc extension support

[sfrench/cifs-2.6.git] / drivers / clocksource / timer-riscv.c

diff --git a/drivers/clocksource/timer-riscv.c b/drivers/clocksource/timer-riscv.c
index 593d5a957b69dcbfe49e134342061497f3ff4231..969a552da8d2971c7df54cfd6af29871a443aff7 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;
 }
 
@@ -101,20 +118,21 @@ static irqreturn_t riscv_timer_interrupt(int irq, void *dev_id)
 
 static int __init riscv_timer_init_dt(struct device_node *n)
 {
-       int cpuid, hartid, error;
+       int cpuid, error;
+       unsigned long hartid;
        struct device_node *child;
        struct irq_domain *domain;
 
-       hartid = riscv_of_processor_hartid(n);
-       if (hartid < 0) {
-               pr_warn("Not valid hartid for node [%pOF] error = [%d]\n",
+       error = riscv_of_processor_hartid(n, &hartid);
+       if (error < 0) {
+               pr_warn("Not valid hartid for node [%pOF] error = [%lu]\n",
                        n, hartid);
-               return hartid;
+               return error;
        }
 
        cpuid = riscv_hartid_to_cpuid(hartid);
        if (cpuid < 0) {
-               pr_warn("Invalid cpuid for hartid [%d]\n", hartid);
+               pr_warn("Invalid cpuid for hartid [%lu]\n", hartid);
                return cpuid;
        }
 
@@ -140,7 +158,7 @@ static int __init riscv_timer_init_dt(struct device_node *n)
                return -ENODEV;
        }
 
-       pr_info("%s: Registering clocksource cpuid [%d] hartid [%d]\n",
+       pr_info("%s: Registering clocksource cpuid [%d] hartid [%lu]\n",
               __func__, cpuid, hartid);
        error = clocksource_register_hz(&riscv_clocksource, riscv_timebase);
        if (error) {
@@ -165,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;
 }