[libcpu][risc-v]Feat: Add OpenSBI SMP support to t-head c908

7 files changed, 267 insertions, 54 deletions

diff --git a/libcpu/risc-v/common64/cpuport.c b/libcpu/risc-v/common64/cpuport.c
index b33da61a38..674208ebf4 100644
--- a/libcpu/risc-v/common64/cpuport.c
+++ b/libcpu/risc-v/common64/cpuport.c
@@ -12,6 +12,7 @@
 
 #include <rthw.h>
 #include <rtthread.h>
+#include <rtatomic.h>
 
 #include "cpuport.h"
 #include "stack.h"
@@ -25,6 +26,8 @@
 #ifdef RT_USING_SMP
 #include "tick.h"
 #include "interrupt.h"
+
+volatile unsigned long rt_riscv_online_mask;
 #endif /* RT_USING_SMP */
 
 #ifdef ARCH_RISCV_FPU
@@ -94,6 +97,40 @@ int rt_hw_cpu_id(void)
 }
 
 /**
+ * @brief Return the one-based position of the least significant set bit.
+ *
+ * Declared in rtthread.h and required by components such as smp_call and the
+ * RT_FIELD_PREP/RT_FIELD_GET helpers; only aarch64 used to provide it.
+ */
+unsigned long __rt_ffsl(unsigned long value)
+{
+#ifdef __GNUC__
+    return __builtin_ffsl(value);
+#else
+    unsigned long bit;
+
+    if (!value)
+    {
+        return 0;
+    }
+
+    for (bit = 1; (value & 1UL) == 0; bit++)
+    {
+        value >>= 1;
+    }
+
+    return bit;
+#endif /* __GNUC__ */
+}
+
+#ifdef RT_USING_SMP
+void rt_hw_cpu_mark_online(void)
+{
+    rt_hw_atomic_or((volatile rt_atomic_t *)&rt_riscv_online_mask, ((rt_atomic_t)1U << rt_hw_cpu_id()));
+}
+#endif /* RT_USING_SMP */
+
+/**
  * This function will initialize thread stack, we assuming
  * when scheduler restore this new thread, context will restore
  * an entry to user first application
diff --git a/libcpu/risc-v/common64/cpuport.h b/libcpu/risc-v/common64/cpuport.h
index 9830b0dc26..e02199e2e5 100644
--- a/libcpu/risc-v/common64/cpuport.h
+++ b/libcpu/risc-v/common64/cpuport.h
@@ -24,6 +24,10 @@ typedef union {
         unsigned short next;
     } tickets;
 } rt_hw_spinlock_t;
+
+extern volatile unsigned long rt_riscv_online_mask;
+
+void rt_hw_cpu_mark_online(void);
 #endif
 
 #include <rtcompiler.h>
diff --git a/libcpu/risc-v/common64/tlb.h b/libcpu/risc-v/common64/tlb.h
index 5697ca61f6..4e0e79322f 100644
--- a/libcpu/risc-v/common64/tlb.h
+++ b/libcpu/risc-v/common64/tlb.h
@@ -18,6 +18,11 @@
 #include "sbi.h"
 #include "riscv_mmu.h"
 
+#ifdef RT_USING_SMP
+#include <rtatomic.h>
+#include "cpuport.h"
+#endif
+
 #define HANDLE_FAULT(ret)                                                      \
     if (__builtin_expect((ret) != SBI_SUCCESS, 0))                             \
         LOG_W("%s failed", __FUNCTION__);
@@ -36,11 +41,31 @@ static inline void rt_hw_tlb_invalidate_all_local(void)
 static inline void rt_hw_tlb_invalidate_aspace(rt_aspace_t aspace)
 {
     // TODO ASID
+#ifdef RT_USING_SMP
+    uintptr_t mask = rt_hw_atomic_load((volatile rt_atomic_t *)&rt_riscv_online_mask);
+
+    if (mask != 0)
+    {
+        HANDLE_FAULT(sbi_remote_sfence_vma(&mask, 0, 0, (unsigned long)-1));
+        return;
+    }
+#endif /* RT_USING_SMP */
+
     rt_hw_tlb_invalidate_all_local();
 }
 
 static inline void rt_hw_tlb_invalidate_page(rt_aspace_t aspace, void *start)
 {
+#ifdef RT_USING_SMP
+    uintptr_t mask = rt_hw_atomic_load((volatile rt_atomic_t *)&rt_riscv_online_mask);
+
+    if (mask != 0)
+    {
+        HANDLE_FAULT(sbi_remote_sfence_vma(&mask, 0, (unsigned long)start, ARCH_PAGE_SIZE));
+        return;
+    }
+#endif /* RT_USING_SMP */
+
     __asm__ volatile("sfence.vma %0, zero" ::"r"(start) : "memory");
 }
 
diff --git a/libcpu/risc-v/t-head/c908/cache.h b/libcpu/risc-v/t-head/c908/cache.h
index 09ebce6861..cf7b21c88a 100644
--- a/libcpu/risc-v/t-head/c908/cache.h
+++ b/libcpu/risc-v/t-head/c908/cache.h
@@ -90,22 +90,6 @@ ALWAYS_INLINE void rt_hw_cpu_icache_invalidate_all_local(void)
  * ========================================
  */
 
-#ifdef RT_USING_SMP
-#error "TODO: cache maintainence have not ported to RISC-V SMP yet"
-
-void rt_hw_cpu_dcache_clean(void *addr, int size);
-void rt_hw_cpu_dcache_invalidate(void *addr, int size);
-void rt_hw_cpu_dcache_clean_invalidate(void *addr, int size);
-
-void rt_hw_cpu_dcache_clean_all(void);
-void rt_hw_cpu_dcache_invalidate_all(void);
-void rt_hw_cpu_dcache_clean_invalidate_all(void);
-
-void rt_hw_cpu_icache_invalidate(void *addr, int size);
-void rt_hw_cpu_icache_invalidate_all(void);
-
-#else /* !RT_USING_SMP */
-
 #define rt_hw_cpu_dcache_clean rt_hw_cpu_dcache_clean_local
 #define rt_hw_cpu_dcache_invalidate rt_hw_cpu_dcache_invalidate_local
 #define rt_hw_cpu_dcache_clean_and_invalidate rt_hw_cpu_dcache_clean_invalidate_local
@@ -117,8 +101,6 @@ void rt_hw_cpu_icache_invalidate_all(void);
 #define rt_hw_cpu_icache_invalidate rt_hw_cpu_icache_invalidate_local
 #define rt_hw_cpu_icache_invalidate_all rt_hw_cpu_icache_invalidate_all_local
 
-#endif /* RT_USING_SMP */
-
 /**
  * @brief Synchronize cache to Point of Unification
  */
diff --git a/libcpu/risc-v/t-head/c908/interrupt.c b/libcpu/risc-v/t-head/c908/interrupt.c
index 339dde808f..7caed0203f 100644
--- a/libcpu/risc-v/t-head/c908/interrupt.c
+++ b/libcpu/risc-v/t-head/c908/interrupt.c
@@ -15,9 +15,20 @@
 #include "riscv.h"
 #include "plic.h"
 
+#ifdef RT_USING_SMP
+#include <rtatomic.h>
+#include <encoding.h>
+#include <sbi.h>
+
+struct rt_irq_desc ipi_desc[RT_MAX_IPI];
+static rt_atomic_t ipi_vectors[RT_CPUS_NR] = { 0 };
+#endif /* RT_USING_SMP */
+
+#ifndef RT_USING_SMP
 extern rt_atomic_t rt_interrupt_nest;
 extern rt_uint32_t rt_interrupt_from_thread, rt_interrupt_to_thread;
 extern rt_uint32_t rt_thread_switch_interrupt_flag;
+#endif
 
 struct rt_irq_desc isr_table[INTERRUPTS_MAX];
 
@@ -42,11 +53,13 @@ void rt_hw_interrupt_init(void)
         isr_table[idx].handler = rt_hw_interrupt_handler;
     }
 
+    #ifndef RT_USING_SMP
     /* init interrupt nest, and context in thread sp */
     rt_interrupt_nest               = 0;
     rt_interrupt_from_thread        = 0;
     rt_interrupt_to_thread          = 0;
     rt_thread_switch_interrupt_flag = 0;
+    #endif
 }
 
 /**
@@ -103,3 +116,159 @@ rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler,
 
     return old_handler;
 }
+
+#ifdef RT_USING_SMP
+
+rt_bool_t rt_hw_interrupt_is_disabled(void)
+{
+    /* Determine the interrupt enable state */
+    rt_ubase_t sstatus;
+    __asm__ volatile("csrr %0, sstatus" : "=r"(sstatus));
+    return (sstatus & SSTATUS_SIE) == 0;
+}
+
+void rt_hw_spin_lock_init(rt_hw_spinlock_t *_lock)
+{
+    _lock->slock = 0;
+}
+
+void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
+{
+    /* Ticket lock on top of the AMO atomic ops: low 16 bits of slock are
+     * the owner, high 16 bits are the next-ticket allocator. */
+    rt_atomic_t prev;
+    rt_atomic_t ticket;
+    rt_atomic_t owner;
+
+    prev = rt_hw_atomic_add((volatile rt_atomic_t *)&lock->slock, (rt_atomic_t)(1UL << 16));
+    ticket = (prev >> 16) & 0xffffUL;
+
+    for (;;)
+    {
+        owner = rt_hw_atomic_load((volatile rt_atomic_t *)&lock->slock) & 0xffffUL;
+        if (owner == ticket)
+        {
+            break;
+        }
+    }
+
+    __asm__ volatile("fence rw, rw" ::: "memory");
+}
+
+void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
+{
+    __asm__ volatile("fence rw, rw" ::: "memory");
+
+    if ((rt_hw_atomic_load((volatile rt_atomic_t *)&lock->slock) & (rt_atomic_t)0xffffUL) == (rt_atomic_t)0xffffUL)
+    {
+        /* owner overflow: clear the owner field, keep the ticket field */
+        rt_hw_atomic_and((volatile rt_atomic_t *)&lock->slock, (rt_atomic_t)0xffff0000UL);
+    }
+    else
+    {
+        rt_hw_atomic_add((volatile rt_atomic_t *)&lock->slock, (rt_atomic_t)1);
+    }
+
+    __asm__ volatile("fence rw, rw" ::: "memory");
+}
+
+void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask)
+{
+    unsigned long hart_mask;
+    int cpuid;
+
+    /* drop harts that do not exist or cannot take an IPI yet */
+    cpu_mask &= RT_CPU_MASK;
+    cpu_mask &= (unsigned int)rt_hw_atomic_load((volatile rt_atomic_t *)&rt_riscv_online_mask);
+
+    if (ipi_vector < 0 || ipi_vector >= RT_MAX_IPI || cpu_mask == 0)
+    {
+        return;
+    }
+
+    for (cpuid = 0; cpuid < RT_CPUS_NR; cpuid++)
+    {
+        if (cpu_mask & (1U << cpuid))
+        {
+            rt_hw_atomic_or(&ipi_vectors[cpuid], ((rt_atomic_t)1U << ipi_vector));
+        }
+    }
+
+    __asm__ volatile("fence rw, rw" ::: "memory");
+
+    hart_mask = cpu_mask;
+    sbi_send_ipi(&hart_mask);
+}
+
+void rt_hw_ipi_init(void)
+{
+    static rt_bool_t desc_inited = RT_FALSE;
+    int cpuid = rt_hw_cpu_id();
+
+    if (!desc_inited)
+    {
+        desc_inited = RT_TRUE;
+        for (int idx = 0; idx < RT_MAX_IPI; idx++)
+        {
+            ipi_desc[idx].handler = RT_NULL;
+            ipi_desc[idx].param = RT_NULL;
+#ifdef RT_USING_INTERRUPT_INFO
+            rt_snprintf(ipi_desc[idx].name, RT_NAME_MAX - 1, "default");
+            ipi_desc[idx].counter = 0;
+#endif
+        }
+    }
+
+    rt_hw_atomic_store(&ipi_vectors[cpuid], 0);
+    set_csr(sie, SIP_SSIP);
+
+    /* the vector table and SSIP are ready, so this hart may now be targeted
+     * by rt_hw_ipi_send() and by a remote sfence.vma */
+    rt_hw_cpu_mark_online();
+}
+
+void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler)
+{
+    if (ipi_vector >= 0 && ipi_vector < RT_MAX_IPI && ipi_isr_handler != RT_NULL)
+    {
+        ipi_desc[ipi_vector].handler = ipi_isr_handler;
+        ipi_desc[ipi_vector].param = RT_NULL;
+    }
+}
+
+void rt_hw_ipi_handler(void)
+{
+    int cpuid = rt_hw_cpu_id();
+
+    for (;;)
+    {
+        rt_atomic_t pending = rt_hw_atomic_exchange(&ipi_vectors[cpuid], 0);
+
+        /* acquire counterpart of the release barrier in rt_hw_ipi_send() */
+        __asm__ volatile("fence r, rw" ::: "memory");
+
+        if (pending == 0)
+        {
+            clear_csr(sip, SIP_SSIP);
+            if (rt_hw_atomic_load(&ipi_vectors[cpuid]) == 0)
+            {
+                break;
+            }
+            continue;
+        }
+
+        clear_csr(sip, SIP_SSIP);
+
+        while (pending)
+        {
+            int bitpos = __builtin_ctzl((unsigned long)pending);
+
+            pending &= ~((rt_atomic_t)1U << bitpos);
+            if (bitpos < RT_MAX_IPI && ipi_desc[bitpos].handler != RT_NULL)
+            {
+                ipi_desc[bitpos].handler(bitpos, ipi_desc[bitpos].param);
+            }
+        }
+    }
+}
+#endif /* RT_USING_SMP */
diff --git a/libcpu/risc-v/t-head/c908/interrupt.h b/libcpu/risc-v/t-head/c908/interrupt.h
index 2ee0824837..d1b7ea13a3 100644
--- a/libcpu/risc-v/t-head/c908/interrupt.h
+++ b/libcpu/risc-v/t-head/c908/interrupt.h
@@ -45,4 +45,12 @@ void rt_hw_interrupt_mask(int vector);
 void rt_hw_interrupt_umask(int vector);
 rt_isr_handler_t rt_hw_interrupt_install(int vector, rt_isr_handler_t handler, void *param, const char *name);
 
+#ifdef RT_USING_SMP
+rt_bool_t rt_hw_interrupt_is_disabled(void);
+void rt_hw_ipi_handler(void);
+void rt_hw_ipi_handler_install(int ipi_vector, rt_isr_handler_t ipi_isr_handler);
+void rt_hw_ipi_init(void);
+void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask);
+#endif /* RT_USING_SMP */
+
 #endif
diff --git a/libcpu/risc-v/t-head/c908/plic.c b/libcpu/risc-v/t-head/c908/plic.c
index 08cb6c18d2..7cb313280d 100644
--- a/libcpu/risc-v/t-head/c908/plic.c
+++ b/libcpu/risc-v/t-head/c908/plic.c
@@ -156,10 +156,11 @@ rt_inline void plic_toggle(struct plic_handler *handler, int hwirq, int enable)
 void plic_init(void)
 {
     int nr_irqs;
-    int nr_context;
     int i;
     unsigned long hwirq;
-    int cpu = 0;
+    struct plic_handler *handler;
+    uint32_t threshold = 0;
+    extern int boot_hartid;
 
     if (plic_regs)
     {
@@ -167,8 +168,6 @@ void plic_init(void)
         return;
     }
 
-    nr_context = C908_NR_CONTEXT;
-
     plic_regs = (void *)C908_PLIC_PHY_ADDR;
     if (!plic_regs)
     {
@@ -178,41 +177,30 @@ void plic_init(void)
 
     nr_irqs = C908_PLIC_NR_EXT_IRQS;
 
-    for (i = 0; i < nr_context; i ++)
-    {
-        struct plic_handler *handler;
-        uint32_t threshold = 0;
-
-        cpu = 0;
-
-        /* skip contexts other than supervisor external interrupt */
-        if (i == 0)
-        {
-            continue;
-        }
-
-        // we always use CPU0 M-mode target register.
-        handler = &plic_handlers[cpu];
-        if (handler->present)
-        {
-            threshold  = 0xffffffff;
-            goto done;
-        }
+    /*
+     * PLIC context layout: hart N owns context 2N (M-mode) and 2N + 1
+     * (S-mode). The kernel only ever takes S-mode external interrupts, and
+     * only on the hart the firmware entered - which is not necessarily hart 0,
+     * as OpenSBI picks the boot hart dynamically. Binding the handler to a
+     * hard-coded context therefore leaves external interrupts masked whenever
+     * the boot hart is not 0, so derive the context from boot_hartid.
+     */
+    i = boot_hartid * C908_NR_CONTEXT + 1;
 
-        handler->present = RT_TRUE;
-        handler->hart_base = (void *)((rt_size_t)plic_regs + CONTEXT_BASE + i * CONTEXT_PER_HART);
-        handler->enable_base = (void *)((rt_size_t)plic_regs + ENABLE_BASE + i * ENABLE_PER_HART);
+    handler = &plic_handlers[0];
+    handler->present = RT_TRUE;
+    handler->hart_base = (void *)((rt_size_t)plic_regs + CONTEXT_BASE + i * CONTEXT_PER_HART);
+    handler->enable_base = (void *)((rt_size_t)plic_regs + ENABLE_BASE + i * ENABLE_PER_HART);
 #ifdef RT_USING_SMART
-        handler->hart_base = (void *)rt_ioremap(handler->hart_base, 0x1000);
-        handler->enable_base = (void *)rt_ioremap(handler->enable_base, 0x1000);
+    handler->hart_base = (void *)rt_ioremap(handler->hart_base, 0x1000);
+    handler->enable_base = (void *)rt_ioremap(handler->enable_base, 0x1000);
 #endif
-done:
-        /* priority must be > threshold to trigger an interrupt */
-        writel(threshold, (void *)((rt_size_t)handler->hart_base + CONTEXT_THRESHOLD));
-        for (hwirq = 1; hwirq <= nr_irqs; hwirq++)
-        {
-            plic_toggle(handler, hwirq, 0);
-        }
+
+    /* priority must be > threshold to trigger an interrupt */
+    writel(threshold, (void *)((rt_size_t)handler->hart_base + CONTEXT_THRESHOLD));
+    for (hwirq = 1; hwirq <= nr_irqs; hwirq++)
+    {
+        plic_toggle(handler, hwirq, 0);
     }
 
     /* Enable supervisor external interrupts. */