add k210 SMP support

6 files changed, 314 insertions, 34 deletions

diff --git a/libcpu/risc-v/common/context_gcc.S b/libcpu/risc-v/common/context_gcc.S
index 94ae5d9df4..62e64d8104 100644
--- a/libcpu/risc-v/common/context_gcc.S
+++ b/libcpu/risc-v/common/context_gcc.S
@@ -6,10 +6,16 @@
  * Change Logs:
  * Date           Author       Notes
  * 2018/10/28     Bernard      The unify RISC-V porting implementation
+ * 2018/12/27     Jesven       Add SMP support
  */
 
 #include "cpuport.h"
 
+#ifdef RT_USING_SMP
+#define rt_hw_interrupt_disable rt_hw_local_irq_disable
+#define rt_hw_interrupt_enable  rt_hw_local_irq_enable
+#endif
+
 /*
  * rt_base_t rt_hw_interrupt_disable(void);
  */
@@ -27,13 +33,23 @@ rt_hw_interrupt_enable:
     ret
 
 /*
- * void rt_hw_context_switch_to(rt_ubase_t to)
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch_to(rt_ubase_t to, stuct rt_thread *to_thread);
+ * #else
+ * void rt_hw_context_switch_to(rt_ubase_t to);
+ * #endif
  * a0 --> to
+ * a1 --> to_thread
  */
     .globl rt_hw_context_switch_to
 rt_hw_context_switch_to:
     LOAD sp, (a0)
 
+#ifdef RT_USING_SMP
+    mv   a0,   a1
+    jal  rt_cpus_lock_status_restore
+#endif
+
     /* load epc from stack */
     LOAD a0,   0 * REGBYTES(sp)
     csrw mepc, a0
@@ -74,9 +90,15 @@ rt_hw_context_switch_to:
     mret
 
 /*
- * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to)
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ * #else
+ * void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to);
+ * #endif
+ *
  * a0 --> from
  * a1 --> to
+ * a2 --> to_thread
  */
     .globl rt_hw_context_switch
 rt_hw_context_switch:
@@ -136,6 +158,78 @@ save_mpie:
      */
     LOAD sp,  (a1)
 
+#ifdef RT_USING_SMP
+    mv   a0,   a2
+    jal  rt_cpus_lock_status_restore
+#endif /*RT_USING_SMP*/
+
+    /* resw ra to mepc */
+    LOAD a1,   0 * REGBYTES(sp)
+    csrw mepc, a1
+    LOAD x1,   1 * REGBYTES(sp)
+
+    /* force to machin mode(MPP=11) */
+    li a1, 0x00001800;
+    csrs mstatus, a1
+    LOAD a1,   2 * REGBYTES(sp)
+    csrs mstatus, a1
+
+    LOAD x4,   4 * REGBYTES(sp)
+    LOAD x5,   5 * REGBYTES(sp)
+    LOAD x6,   6 * REGBYTES(sp)
+    LOAD x7,   7 * REGBYTES(sp)
+    LOAD x8,   8 * REGBYTES(sp)
+    LOAD x9,   9 * REGBYTES(sp)
+    LOAD x10, 10 * REGBYTES(sp)
+    LOAD x11, 11 * REGBYTES(sp)
+    LOAD x12, 12 * REGBYTES(sp)
+    LOAD x13, 13 * REGBYTES(sp)
+    LOAD x14, 14 * REGBYTES(sp)
+    LOAD x15, 15 * REGBYTES(sp)
+    LOAD x16, 16 * REGBYTES(sp)
+    LOAD x17, 17 * REGBYTES(sp)
+    LOAD x18, 18 * REGBYTES(sp)
+    LOAD x19, 19 * REGBYTES(sp)
+    LOAD x20, 20 * REGBYTES(sp)
+    LOAD x21, 21 * REGBYTES(sp)
+    LOAD x22, 22 * REGBYTES(sp)
+    LOAD x23, 23 * REGBYTES(sp)
+    LOAD x24, 24 * REGBYTES(sp)
+    LOAD x25, 25 * REGBYTES(sp)
+    LOAD x26, 26 * REGBYTES(sp)
+    LOAD x27, 27 * REGBYTES(sp)
+    LOAD x28, 28 * REGBYTES(sp)
+    LOAD x29, 29 * REGBYTES(sp)
+    LOAD x30, 30 * REGBYTES(sp)
+    LOAD x31, 31 * REGBYTES(sp)
+
+    addi sp,  sp, 32 * REGBYTES
+    mret
+
+#ifdef RT_USING_SMP
+/*
+ * void rt_hw_context_switch_interrupt(void *context, rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ *
+ * a0 --> context
+ * a1 --> from
+ * a2 --> to
+ * a3 --> to_thread
+ */
+    .globl rt_hw_context_switch_interrupt
+rt_hw_context_switch_interrupt:
+
+    STORE a0, 0(a1)
+
+    csrr  a1, mepc
+    STORE a1, 0 * REGBYTES(a0)
+
+    csrr  a1, mstatus
+    STORE a1, 2 * REGBYTES(a0)
+
+    LOAD  sp, 0(a2)
+    move  a0, a3
+    call rt_cpus_lock_status_restore
+
     /* resw ra to mepc */
     LOAD a1,   0 * REGBYTES(sp)
     csrw mepc, a1
@@ -178,3 +272,5 @@ save_mpie:
 
     addi sp,  sp, 32 * REGBYTES
     mret
+
+#endif
diff --git a/libcpu/risc-v/common/cpuport.c b/libcpu/risc-v/common/cpuport.c
index 2ad77b99c5..a44dc40f79 100644
--- a/libcpu/risc-v/common/cpuport.c
+++ b/libcpu/risc-v/common/cpuport.c
@@ -13,9 +13,11 @@
 
 #include "cpuport.h"
 
+#ifndef RT_USING_SMP
 volatile rt_ubase_t  rt_interrupt_from_thread = 0;
 volatile rt_ubase_t  rt_interrupt_to_thread   = 0;
 volatile rt_uint32_t rt_thread_switch_interrupt_flag = 0;
+#endif
 
 struct rt_hw_stack_frame
 {
@@ -93,6 +95,14 @@ rt_uint8_t *rt_hw_stack_init(void       *tentry,
     return stk;
 }
 
+/*
+ * #ifdef RT_USING_SMP
+ * void rt_hw_context_switch_interrupt(void *context, rt_ubase_t from, rt_ubase_t to, struct rt_thread *to_thread);
+ * #else
+ * void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to);
+ * #endif
+ */
+#ifndef RT_USING_SMP
 void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to)
 {
     if (rt_thread_switch_interrupt_flag == 0)
@@ -103,3 +113,17 @@ void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to)
 
     return ;
 }
+#endif /* end of RT_USING_SMP */
+
+/** shutdown CPU */
+void rt_hw_cpu_shutdown()
+{
+    rt_uint32_t level;
+    rt_kprintf("shutdown...\n");
+
+    level = rt_hw_interrupt_disable();
+    while (level)
+    {
+        RT_ASSERT(0);
+    }
+}
diff --git a/libcpu/risc-v/k210/cpuport_smp.c b/libcpu/risc-v/k210/cpuport_smp.c
new file mode 100644
index 0000000000..a31314a13f
--- /dev/null
+++ b/libcpu/risc-v/k210/cpuport_smp.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2018/12/23     Bernard      The first version
+ * 2018/12/27     Jesven       Add secondary cpu boot
+ */
+
+#include <rthw.h>
+#include <rtthread.h>
+#include <stdint.h>
+
+#include "board.h"
+#include <encoding.h>
+#include <clint.h>
+#include <atomic.h>
+
+#ifdef RT_USING_SMP
+
+int rt_hw_cpu_id(void)
+{
+    return read_csr(mhartid);
+}
+
+void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
+{
+    spinlock_lock((spinlock_t *)lock);
+}
+
+void rt_hw_spin_unlock(rt_hw_spinlock_t *lock)
+{
+    spinlock_unlock((spinlock_t *)lock);
+}
+
+void rt_hw_ipi_send(int ipi_vector, unsigned int cpu_mask)
+{
+    int idx;
+
+    for (idx = 0; idx < RT_CPUS_NR; idx ++)
+    {
+        if (cpu_mask & (1 << idx))
+        {
+            clint_ipi_send(idx);
+        }
+    }
+}
+
+extern rt_base_t secondary_boot_flag;
+void rt_hw_secondary_cpu_up(void)
+{
+    mb();
+    secondary_boot_flag = 0xa55a;
+}
+
+extern void rt_hw_scondary_interrupt_init(void);
+extern int rt_hw_tick_init(void);
+extern int rt_hw_clint_ipi_enable(void);
+
+void secondary_cpu_c_start(void)
+{
+    rt_hw_spin_lock(&_cpus_lock);
+
+    /* initialize interrupt controller */
+    rt_hw_scondary_interrupt_init();
+
+    rt_hw_tick_init();
+
+    rt_hw_clint_ipi_enable();
+
+    rt_system_scheduler_start();
+}
+
+void rt_hw_secondary_cpu_idle_exec(void)
+{
+    asm volatile ("wfi");
+}
+#endif /*RT_USING_SMP*/
diff --git a/libcpu/risc-v/k210/interrupt.c b/libcpu/risc-v/k210/interrupt.c
index f9c3eb5a67..3d30e69ed2 100644
--- a/libcpu/risc-v/k210/interrupt.c
+++ b/libcpu/risc-v/k210/interrupt.c
@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2018/10/01     Bernard      The first version
+ * 2018/12/27     Jesven       Change irq enable/disable to cpu0
  */
 
 #include <rthw.h>
@@ -19,7 +20,7 @@
 #define CPU_NUM         2
 #define MAX_HANDLERS    IRQN_MAX
 
-static struct rt_irq_desc irq_desc[CPU_NUM][MAX_HANDLERS];
+static struct rt_irq_desc irq_desc[MAX_HANDLERS];
 
 static rt_isr_handler_t rt_hw_interrupt_handle(rt_uint32_t vector, void *param)
 {
@@ -27,6 +28,52 @@ static rt_isr_handler_t rt_hw_interrupt_handle(rt_uint32_t vector, void *param)
     return RT_NULL;
 }
 
+int rt_hw_clint_ipi_enable(void)
+{
+    /* Set the Machine-Software bit in MIE */
+    set_csr(mie, MIP_MSIP);
+    return 0;
+}
+
+int rt_hw_clint_ipi_disable(void)
+{
+    /* Clear the Machine-Software bit in MIE */
+    clear_csr(mie, MIP_MSIP);
+    return 0;
+}
+
+int rt_hw_plic_irq_enable(plic_irq_t irq_number)
+{
+    unsigned long core_id = 0;
+
+    /* Check parameters */
+    if (PLIC_NUM_SOURCES < irq_number || 0 > irq_number)
+        return -1;
+    /* Get current enable bit array by IRQ number */
+    uint32_t current = plic->target_enables.target[core_id].enable[irq_number / 32];
+    /* Set enable bit in enable bit array */
+    current |= (uint32_t)1 << (irq_number % 32);
+    /* Write back the enable bit array */
+    plic->target_enables.target[core_id].enable[irq_number / 32] = current;
+    return 0;
+}
+
+int rt_hw_plic_irq_disable(plic_irq_t irq_number)
+{
+    unsigned long core_id = 0;
+
+    /* Check parameters */
+    if (PLIC_NUM_SOURCES < irq_number || 0 > irq_number)
+        return -1;
+    /* Get current enable bit array by IRQ number */
+    uint32_t current = plic->target_enables.target[core_id].enable[irq_number / 32];
+    /* Clear enable bit in enable bit array */
+    current &= ~((uint32_t)1 << (irq_number % 32));
+    /* Write back the enable bit array */
+    plic->target_enables.target[core_id].enable[irq_number / 32] = current;
+    return 0;
+}
+
 /**
  * This function will initialize hardware interrupt
  */
@@ -52,11 +99,11 @@ void rt_hw_interrupt_init(void)
     for (idx = 0; idx < MAX_HANDLERS; idx++)
     {
         rt_hw_interrupt_mask(idx);
-        irq_desc[cpuid][idx].handler = (rt_isr_handler_t)rt_hw_interrupt_handle;
-        irq_desc[cpuid][idx].param = RT_NULL;
+        irq_desc[idx].handler = (rt_isr_handler_t)rt_hw_interrupt_handle;
+        irq_desc[idx].param = RT_NULL;
 #ifdef RT_USING_INTERRUPT_INFO
-        rt_snprintf(irq_desc[cpuid][idx].name, RT_NAME_MAX - 1, "default");
-        irq_desc[idx][cpuid].counter = 0;
+        rt_snprintf(irq_desc[idx].name, RT_NAME_MAX - 1, "default");
+        irq_desc[idx].counter = 0;
 #endif
     }
 
@@ -64,13 +111,31 @@ void rt_hw_interrupt_init(void)
     set_csr(mie, MIP_MEIP);
 }
 
+void rt_hw_scondary_interrupt_init(void)
+{
+    int idx;
+    int cpuid;
+
+    cpuid = current_coreid();
+
+    /* Disable all interrupts for the current core. */
+    for (idx = 0; idx < ((PLIC_NUM_SOURCES + 32u) / 32u); idx ++)
+        plic->target_enables.target[cpuid].enable[idx] = 0;
+
+    /* Set the threshold to zero. */
+    plic->targets.target[cpuid].priority_threshold = 0;
+
+    /* Enable machine external interrupts. */
+    set_csr(mie, MIP_MEIP);
+}
+
 /**
  * This function will mask a interrupt.
  * @param vector the interrupt number
  */
 void rt_hw_interrupt_mask(int vector)
 {
-    plic_irq_disable(vector);
+    rt_hw_plic_irq_disable(vector);
 }
 
 /**
@@ -80,7 +145,7 @@ void rt_hw_interrupt_mask(int vector)
 void rt_hw_interrupt_umask(int vector)
 {
     plic_set_priority(vector, 1);
-    plic_irq_enable(vector);
+    rt_hw_plic_irq_enable(vector);
 }
 
 /**
@@ -92,21 +157,18 @@ 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)
 {
-    int cpuid;
     rt_isr_handler_t old_handler = RT_NULL;
 
-    cpuid = current_coreid();
-
     if(vector < MAX_HANDLERS)
     {
-        old_handler = irq_desc[cpuid][vector].handler;
+        old_handler = irq_desc[vector].handler;
         if (handler != RT_NULL)
         {
-            irq_desc[cpuid][vector].handler = (rt_isr_handler_t)handler;
-            irq_desc[cpuid][vector].param = param;
+            irq_desc[vector].handler = (rt_isr_handler_t)handler;
+            irq_desc[vector].param = param;
 #ifdef RT_USING_INTERRUPT_INFO
-            rt_snprintf(irq_desc[cpuid][vector].name, RT_NAME_MAX - 1, "%s", name);
-            irq_desc[cpuid][vector].counter = 0;
+            rt_snprintf(irq_desc[vector].name, RT_NAME_MAX - 1, "%s", name);
+            irq_desc[vector].counter = 0;
 #endif
         }
     }
@@ -149,14 +211,14 @@ uintptr_t handle_irq_m_ext(uintptr_t cause, uintptr_t epc)
         /* Disable software interrupt and timer interrupt */
         clear_csr(mie, MIP_MTIP | MIP_MSIP);
 
-        if (irq_desc[core_id][int_num].handler == (rt_isr_handler_t)rt_hw_interrupt_handle)
+        if (irq_desc[int_num].handler == (rt_isr_handler_t)rt_hw_interrupt_handle)
         {
             /* default handler, route to kendryte bsp plic driver */
             plic_irq_handle(int_num);
         }
-        else if (irq_desc[core_id][int_num].handler)
+        else if (irq_desc[int_num].handler)
         {
-            irq_desc[core_id][int_num].handler(int_num, irq_desc[core_id][int_num].param);
+            irq_desc[int_num].handler(int_num, irq_desc[int_num].param);
         }
 
         /* Perform IRQ complete */
@@ -185,6 +247,12 @@ uintptr_t handle_trap(uintptr_t mcause, uintptr_t epc)
         switch (cause)
         {
             case IRQ_M_SOFT:
+                {
+                    uint64_t core_id = current_coreid();
+
+                    clint_ipi_clear(core_id);
+                    rt_schedule();
+                }
                 break;
             case IRQ_M_EXT:
                 handle_irq_m_ext(mcause, epc);
diff --git a/libcpu/risc-v/k210/interrupt_gcc.S b/libcpu/risc-v/k210/interrupt_gcc.S
index 5783322237..5d3e8368b1 100644
--- a/libcpu/risc-v/k210/interrupt_gcc.S
+++ b/libcpu/risc-v/k210/interrupt_gcc.S
@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2018/10/02     Bernard      The first version
+ * 2018/12/27     Jesven       Add SMP schedule
  */
 
 #include "cpuport.h"
@@ -72,6 +73,13 @@ trap_entry:
     call  handle_trap
     call  rt_interrupt_leave
 
+#ifdef RT_USING_SMP
+    /* s0 --> sp */
+    mv  a0, s0
+    call rt_scheduler_do_irq_switch
+    mv  sp, s0
+#else
+
     /* switch to from_thread stack */
     move  sp, s0
 
@@ -94,6 +102,7 @@ trap_entry:
 
     LOAD  a0,  0 * REGBYTES(sp)
     csrw  mepc, a0
+#endif
 
 spurious_interrupt:
     LOAD  x1,   1 * REGBYTES(sp)
diff --git a/libcpu/risc-v/k210/startup_gcc.S b/libcpu/risc-v/k210/startup_gcc.S
index 2e69c5021e..f5c0d8fa94 100644
--- a/libcpu/risc-v/k210/startup_gcc.S
+++ b/libcpu/risc-v/k210/startup_gcc.S
@@ -6,6 +6,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2018/10/01     Bernard      The first version
+ * 2018/12/27     Jesven       Add SMP support
  */
 
 #define MSTATUS_FS      0x00006000U /* initial state of FPU     */
@@ -114,20 +115,22 @@ _start:
   add  sp, sp, t1 /* sp = (cpuid + 1) * __STACKSIZE__ + __stack_start__ */
 
   /* other cpu core, jump to cpu entry directly */
-  bnez a0, _cpu_entry
+  bnez a0, secondary_cpu_entry
+  j primary_cpu_entry
 
-#if 0
-  /* clear bss section */
-  la a0, __bss_start
-  la a1, __bss_end
-  bgeu a0, a1, 2f
-1:
-  sw zero, (a0)
-  addi a0, a0, REGBYTES
-  bltu a0, a1, 1b
-2:
+secondary_cpu_entry:
+#ifdef RT_USING_SMP
+  la a0, secondary_boot_flag
+  ld a0, 0(a0)
+  li a1, 0xa55a
+  beq a0, a1, secondary_cpu_c_start
 #endif
+  j secondary_cpu_entry
 
-_cpu_entry:
-  csrr a0, mhartid
-  j cpu_entry
+#ifdef RT_USING_SMP
+.data
+.global secondary_boot_flag
+.align 3
+secondary_boot_flag:
+    .dword 0
+#endif