arch/riscv: Boot secondary CPUs for SMP support

Secondary CPUs are now initialised and made available to the system. If
the system has more CPUs than configured via CONFIG_MP_NUM_CPUS, those
are still left looping as before.

Some implementations of `soc_interrupt_init` also changed to use
`arch_irq_lock` instead of `irq_lock`.

Signed-off-by: Ederson de Souza <ederson.desouza@intel.com>

arch/riscv/core/CMakeLists.txt

@@ -13,6 +13,7 @@ zephyr_library_sources(
   reboot.c
   reset.S
   switch.S
+  smp.c
   thread.c
 )
 

arch/riscv/core/reset.S

@@ -15,6 +15,9 @@ GTEXT(__reset)
 
 /* imports */
 GTEXT(_PrepC)
+GTEXT(riscv_cpu_wake_flag)
+GTEXT(riscv_cpu_sp)
+GTEXT(z_riscv_secondary_cpu_init)
 
 #if CONFIG_INCLUDE_RESET_VECTOR
 SECTION_FUNC(reset, __reset)
@@ -33,18 +36,17 @@ SECTION_FUNC(reset, __reset)
  * the C domain
  */
 SECTION_FUNC(TEXT, __initialize)
-	/*
-	 * This will boot master core, just halt other cores.
-	 * Note: need to be updated for complete SMP support
-	 */
 	csrr a0, mhartid
-	beqz a0, boot_master_core
+	beqz a0, boot_first_core
 
-loop_slave_core:
+	li t0, CONFIG_MP_NUM_CPUS
+	blt a0, t0, boot_secondary_core
+
+loop_unconfigured_cores:
 	wfi
-	j loop_slave_core
+	j loop_unconfigured_cores
 
-boot_master_core:
+boot_first_core:
 
 #ifdef CONFIG_FPU
 	/*
@@ -93,3 +95,16 @@ aa_loop:
 	 * and then enters kernel z_cstart
 	 */
 	call _PrepC
+
+boot_secondary_core:
+	la t0, riscv_cpu_wake_flag
+	RV_OP_LOADREG t0, 0x00(t0)
+	bne a0, t0, boot_secondary_core
+
+	/* Set up stack */
+	la t0, riscv_cpu_sp
+	RV_OP_LOADREG sp, 0x00(t0)
+
+	la t0, riscv_cpu_wake_flag
+	RV_OP_STOREREG x0, 0x00(t0)
+	j z_riscv_secondary_cpu_init

arch/riscv/core/smp.c

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2021 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel.h>
+
+volatile struct {
+	arch_cpustart_t fn;
+	void *arg;
+} riscv_cpu_init[CONFIG_MP_NUM_CPUS];
+
+volatile uintptr_t riscv_cpu_wake_flag;
+volatile void *riscv_cpu_sp;
+
+void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz,
+		    arch_cpustart_t fn, void *arg)
+{
+	riscv_cpu_init[cpu_num].fn = fn;
+	riscv_cpu_init[cpu_num].arg = arg;
+
+	riscv_cpu_sp = Z_THREAD_STACK_BUFFER(stack) + sz;
+	riscv_cpu_wake_flag = cpu_num;
+
+	while (riscv_cpu_wake_flag != 0U) {
+		;
+	}
+}
+
+void z_riscv_secondary_cpu_init(int cpu_num)
+{
+#if defined(CONFIG_RISCV_SOC_INTERRUPT_INIT)
+	soc_interrupt_init();
+#endif
+#ifdef CONFIG_PMP_STACK_GUARD
+	z_riscv_configure_interrupt_stack_guard();
+#endif
+	riscv_cpu_init[cpu_num].fn(riscv_cpu_init[cpu_num].arg);
+}

soc/riscv/riscv-privilege/common/soc_common_irq.c

@@ -107,7 +107,7 @@ int arch_irq_is_enabled(unsigned int irq)
 __weak void soc_interrupt_init(void)
 {
 	/* ensure that all interrupts are disabled */
-	(void)irq_lock();
+	(void)arch_irq_lock();
 
 	__asm__ volatile ("csrwi mie, 0\n"
 			  "csrwi mip, 0\n");

soc/riscv/riscv-privilege/neorv32/soc.c

@@ -10,7 +10,7 @@
 #if defined(CONFIG_RISCV_SOC_INTERRUPT_INIT)
 void soc_interrupt_init(void)
 {
-	(void)irq_lock();
+	(void)arch_irq_lock();
 
 	__asm__ volatile ("csrwi mie, 0\n");
 }