port/moonlight/src/bootup.c

@@ -9,6 +9,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <picotls.h>
+#include <aclint_ipi.h>
 #include <tx_port.h>
 #include <riscv-csr.h>
 #ifdef __cplusplus
@@ -16,6 +17,9 @@
 #else
 #define EXTERN_C extern
 #endif 
+#if defined(TX_THREAD_SMP_MAX_CORES) && (TX_THREAD_SMP_MAX_CORES > 1)
+#define BOOTUP_SMP_ENABLED
+#endif
 
 // Generic C function pointer.
 typedef void(*function_t)(void) ;
@@ -45,11 +49,58 @@ EXTERN_C void _start(void)  __attribute__ ((naked,section(".text.boot")));
 
 // Entry and exit points as C functions.
 EXTERN_C void _initialize(void) __attribute__ ((noreturn,section(".text.boot")));
+EXTERN_C void _secondary_sleep_forever(void) __attribute__ ((noreturn,section(".text.boot")));
 EXTERN_C void _exit(int exit_code) __attribute__ ((noreturn,noinline,weak));
 
 // Standard entry point, no arguments.
 extern int main(void);
 
+#if defined BOOTUP_SMP_ENABLED
+EXTERN_C void _secondary_initialize(void) __attribute__ ((noreturn,section(".text.boot")));
+EXTERN_C void _secondary_ipi_trap(void) __attribute__ ((naked,noreturn,section(".text.boot")));
+EXTERN_C void _tx_thread_smp_initialize_wait(void) __attribute__ ((noreturn));
+
+void _secondary_initialize(void) {
+    csr_write_mtvec((uint_xlen_t)_secondary_ipi_trap);
+    csr_set_bits_mie(MIE_MSI_BIT_MASK);
+    csr_set_bits_mstatus(MSTATUS_MIE_BIT_MASK);
+
+    __asm__ volatile ("wfi");
+
+    csr_clr_bits_mstatus(MSTATUS_MIE_BIT_MASK);
+
+    _tx_thread_smp_initialize_wait();
+}
+
+void _secondary_ipi_trap(void) {
+#if __riscv_xlen == 64
+    __asm__ volatile ("addi sp, sp, -16;"
+                     "sd   ra, 8(sp);"
+                     "call bootup_ipi_clear_handler;"
+                     "ld   ra, 8(sp);"
+                     "addi sp, sp, 16;"
+                     "mret");
+#else
+    __asm__ volatile ("addi sp, sp, -8;"
+                     "sw   ra, 4(sp);"
+                     "call bootup_ipi_clear_handler;"
+                     "lw   ra, 4(sp);"
+                     "addi sp, sp, 8;"
+                     "mret");
+#endif
+}
+
+static void bootup_wake_secondary_cores(void) {
+    for (UINT core = 1; core < TX_THREAD_SMP_MAX_CORES; ++core) {
+        send_ipi(core);
+    }
+}
+
+__attribute__((used)) void bootup_ipi_clear_handler(void) {
+    set_aclint_msip(aclint, csr_read_mhartid(), 0);
+}
+#endif
+
 // The linker script will place this in the reset entry point.
 // It will be 'called' with no stack or C runtime configuration.
 // tp will not be initialized
@@ -61,7 +112,7 @@ void _start(void) {
         // The 'norelax' option is critical here.
         // Without 'norelax' the global pointer will
         // be loaded relative to the global pointer!
-         ".option norelax;"
+        ".option norelax;"
         "la    gp, __global_pointer$;"
         ".option pop;"
         "la    sp, _sp;"
@@ -74,15 +125,20 @@ void _start(void) {
         "la t1, __stack_size;"
         "la sp, _sp;"
         // Loop incase M extension is not present
+        "mv t2, t0;"
         "1:;"
-        "beqz t0, 2f;"
+        "beqz t2, 2f;"
         "sub sp, sp, t1;"
-        "addi t0, t0, -1;"
+        "addi t2, t2, -1;"
         "j 1b;"
         "2:;"
-#ifdef TX_THREAD_SMP_MAX_CORES
-        "call _tx_thread_smp_initialize_wait;"
+        "beqz t0, 3f;"
+#ifdef BOOTUP_SMP_ENABLED
+        "jal   zero, _secondary_initialize;"
+#else
+        "jal   zero, _secondary_sleep_forever;"
 #endif
+        "3:;"
         "jal   zero, _initialize;"
         : /* output: none %0 */
         : /* input: none */
@@ -107,6 +163,9 @@ void _initialize(void) {
          ++entry) {
         (*entry)();
     }
+#ifdef BOOTUP_SMP_ENABLED
+    bootup_wake_secondary_cores();
+#endif
 #ifdef __THREAD_LOCAL_STORAGE
     _set_tls(__tls_base)
 #endif
@@ -120,6 +179,14 @@ void _initialize(void) {
     _exit(rc);
 }
 
+void _secondary_sleep_forever(void) {
+    csr_clr_bits_mie(MIE_MTI_BIT_MASK);
+    csr_clr_bits_mstatus(MSTATUS_MIE_BIT_MASK);
+    while (1) {
+        __asm__ volatile ("wfi");
+    }
+}
+
 // This should never be called. Report the exit code through HTIF and idle the CPU.
 void _exit(int exit_code) {
     uintptr_t htif_exit_code = (((uintptr_t)(unsigned int)exit_code) << 1) | 1u;