diff --git a/port/threadx_smp/inc/tx_port.h b/port/threadx_smp/inc/tx_port.h index 5553645..aefe3fd 100644 --- a/port/threadx_smp/inc/tx_port.h +++ b/port/threadx_smp/inc/tx_port.h @@ -112,6 +112,10 @@ TX_THREAD_SMP_MAX_CORES. See tx_thread_smp_low_level_initialize.S #define TX_THREAD_SMP_INTER_CORE_INTERRUPT +/* Use default wakeup logic*/ + +#define TX_THREAD_SMP_DEFAULT_WAKEUP_LOGIC + /* Determine if there is customer-specific wakeup logic needed. */ #ifdef TX_THREAD_SMP_WAKEUP_LOGIC diff --git a/port/threadx_smp/src/tx_thread_smp_initialize_wait.S b/port/threadx_smp/src/tx_thread_smp_initialize_wait.S index db78833..1f4e459 100644 --- a/port/threadx_smp/src/tx_thread_smp_initialize_wait.S +++ b/port/threadx_smp/src/tx_thread_smp_initialize_wait.S @@ -1,23 +1,71 @@ #include "tx_port.h" - +#include "csr.h" .section .text .align 2 .global _tx_thread_smp_initialize_wait .type _tx_thread_smp_initialize_wait, @function + .extern _tx_thread_schedule _tx_thread_smp_initialize_wait: - /* Core 0 continues initialization. All other harts wait until the - release flag is set by the low-level SMP initialization path. */ + /* Lockout interrupts while startup synchronization is in progress. */ + csrci mstatus, 0x08 // Lockout interrupts + + /* Pickup current hart ID. */ csrr t0, mhartid // Pickup current hart ID beqz t0, _tx_thread_smp_initialize_done // Core 0 does not wait - la t1, _tx_thread_smp_release_cores_flag // Release flag address + /* Build per-hart offsets for ULONG and pointer arrays. */ + slli t1, t0, 2 // ULONG array offset + slli t2, t0, LOG_REGBYTES // Pointer array offset -_tx_thread_smp_initialize_wait_loop: - LWU t2, 0(t1) // Pickup release flag - bnez t2, _tx_thread_smp_initialize_done // Exit once core 0 releases secondaries - j _tx_thread_smp_initialize_wait_loop // Keep waiting + /* Wait until ThreadX has acknowledged this hart by setting its + system state to TX_INITIALIZE_IN_PROGRESS. */ + li t3, 0xF0F0F0F0 // TX_INITIALIZE_IN_PROGRESS + la t4, _tx_thread_system_state // Base of system state array + add t5, t4, t1 // This hart's system state slot + +_tx_thread_smp_wait_for_initialize: + LWU t6, 0(t5) // Pickup current hart's system state + bne t6, t3, _tx_thread_smp_wait_for_initialize + + /* Save the system stack pointer for this hart. */ + la t3, _tx_thread_system_stack_ptr // Base of system stack pointer array + add t3, t3, t2 // Select this hart's slot + STORE sp, 0(t3) // Save system stack pointer + + /* Wait for core 0 to release the secondary harts. */ + la t3, _tx_thread_smp_release_cores_flag // Release flag address + +_tx_thread_smp_wait_for_release: + LWU t6, 0(t3) // Pickup release flag + beqz t6, _tx_thread_smp_wait_for_release + + /* Acknowledge the release by clearing this hart's system state. */ + sw x0, 0(t5) // Set this hart's system state to zero + + /* Wait for core 0 to finish initialization. */ +_tx_thread_smp_wait_for_core0: + LWU t6, 0(t4) // Pickup core 0 system state + bnez t6, _tx_thread_smp_wait_for_core0 + + /* Prepare interrupt state */ + li t0, MSTATUS_MIE + csrrc zero, mstatus, t0 // clear MSTATUS_MIE bit + li t0, (MSTATUS_MPP_M | MSTATUS_MPIE ) + csrrs zero, mstatus, t0 // set MSTATUS_MPP, MPIE bit + li t0, (MIE_MTIE | MIE_MSIE | MIE_MEIE) + csrrs zero, mie, t0 // set mie +#ifdef __riscv_flen + li t0, MSTATUS_FS + csrrs zero, mstatus, t0 // set MSTATUS_FS bit to open f/d isa in riscv + fscsr x0 +#endif + la t0, trap_entry + csrw mtvec, t0 + + /* Initialization is complete for this hart, enter the scheduler. */ + j _tx_thread_schedule _tx_thread_smp_initialize_done: ret diff --git a/port/threadx_smp/src/tx_thread_smp_low_level_initialize.S b/port/threadx_smp/src/tx_thread_smp_low_level_initialize.S index f2f7357..fb8536e 100644 --- a/port/threadx_smp/src/tx_thread_smp_low_level_initialize.S +++ b/port/threadx_smp/src/tx_thread_smp_low_level_initialize.S @@ -6,26 +6,4 @@ .global _tx_thread_smp_low_level_initialize .type _tx_thread_smp_low_level_initialize, @function _tx_thread_smp_low_level_initialize: - - /* Only hart 0 performs low-level SMP initialization. */ - csrr t0, mhartid // Pickup current hart ID - bnez t0, _tx_thread_smp_low_level_initialize_done - - /* Keep secondary harts parked until kernel-enter releases them. */ - la t1, _tx_thread_smp_release_cores_flag - sw x0, 0(t1) // Clear release flag - -#ifdef TX_THREAD_SMP_DYNAMIC_CORE_MAX - /* Record the detected core count supplied by the caller. */ - la t1, _tx_thread_smp_detected_cores - sw a0, 0(t1) -#endif - - /* Platform-specific secondary-hart startup can go here. - This is where core 0 would program boot addresses, send startup IPIs, - or otherwise bring the other harts online. */ - - fence rw, rw // Publish startup state - -_tx_thread_smp_low_level_initialize_done: ret