OS/ThreadX: Fix SMP scheduler SWI context handling

Track normal IRQ nesting in the per-core ThreadX system state, while
keeping the scheduler SWI path separate from normal ISR dispatch.

Also make PortThreadSwitch claim the selected ready thread and restore
that same thread from _tx_thread_current_ptr, avoiding a later reload of
_tx_thread_execute_ptr that may have been changed by another hart.

Signed-off-by: Huaqi Fang <578567190@qq.com>

Author: fanghuaqi

OS/ThreadX/ports_smp/nuclei/gcc/context.S

@@ -63,7 +63,7 @@ _tx_thread_schedule:
     addi  t0, t0, 1
     STORE t0, 1 * REGBYTES(a0)
 
-    LOAD sp,  2 * REGBYTES(a0)      /* Read sp from _tx_thread_execute_ptr[coreid] -> tx_thread_stack_ptr */
+    LOAD sp,  2 * REGBYTES(a0)      /* Read sp from selected thread -> tx_thread_stack_ptr */
     /* Pop PC from stack and set MEPC */
     LOAD t0,  0  * REGBYTES(sp)
     csrw CSR_MEPC, t0
@@ -147,10 +147,12 @@ eclic_msip_handler:
     /* Push mstatus to stack */
     csrr t0, CSR_MSTATUS
     STORE t0,  (portRegNum - 1)  * REGBYTES(sp)
+    /* Store last pc to thread stack */
+    csrr t0, CSR_MEPC
+    STORE t0, 0(sp)
 
     /* Push additional registers */
 
-    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be saved */
 #if defined(SMP_CPU_CNT) && (SMP_CPU_CNT > 1)
     la t0, _tx_thread_current_ptr
     csrr t1, CSR_MHARTID
@@ -160,27 +162,30 @@ eclic_msip_handler:
     la t0, _tx_thread_current_ptr
 #endif
     LOAD t0, 0(t0)
-    beqz t0, _tx_thread_switch
+    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be saved */
+    beqz t0, _tx_thread_save_switch
     /* Store sp to task stack to _tx_thread_current_ptr -> tx_thread_stack_ptr */
     STORE sp, 2 * REGBYTES(t0)
-    /* Store last pc to thread stack */
-    csrr t0, CSR_MEPC
-    STORE t0, 0(sp)
 
-_tx_thread_switch:
+_tx_thread_save_switch:
     jal PortThreadSwitch
 
-    /* Switch task context to _tx_thread_execute_ptr[coreid] */
+    /* PortThreadSwitch has claimed and published the selected thread in
+       _tx_thread_current_ptr[coreid].  Restore that exact thread instead of
+       reloading _tx_thread_execute_ptr, which may be changed by another hart. */
 #if defined(SMP_CPU_CNT) && (SMP_CPU_CNT > 1)
-    la t0, _tx_thread_execute_ptr
+    la t0, _tx_thread_current_ptr
     csrr t1, CSR_MHARTID
     slli t1, t1, LOG_REGBYTES
     add t0, t0, t1
 #else
-    la t0, _tx_thread_execute_ptr
+    la t0, _tx_thread_current_ptr
 #endif
     LOAD t0, 0(t0)
+    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be restored */
+    beqz t0, _tx_thread_restore_switch
     LOAD sp, 2 * REGBYTES(t0)
+_tx_thread_restore_switch:
 
     /* Pop PC from stack and set MEPC */
     LOAD t0,  0  * REGBYTES(sp)

OS/ThreadX/ports_smp/nuclei/gcc/interrupt.S

@@ -15,6 +15,19 @@
     csrc CSR_MSTATUS, MSTATUS_MIE
 .endm
 
+/* Update ThreadX SMP per-core ISR nesting state.
+ * This macro must be called after SAVE_CONTEXT because it clobbers t0-t2.
+ */
+.macro UPDATE_THREADX_SYSTEM_STATE delta
+    la t0, _tx_thread_system_state
+    csrr t1, CSR_MHARTID
+    slli t1, t1, LOG_REGBYTES
+    add t0, t0, t1
+    LOAD t2, 0(t0)
+    addi t2, t2, \delta
+    STORE t2, 0(t0)
+.endm
+
 /**
  * \brief  Macro for context save
  * \details
@@ -202,6 +215,11 @@ irq_entry:
     /* Save the necessary CSR registers */
     SAVE_CSR_CONTEXT
 
+    /* Mark this core as being in a normal IRQ handler.
+     * eclic_msip_handler is the scheduler SWI path and is handled separately.
+     */
+    UPDATE_THREADX_SYSTEM_STATE 1
+
     /* This special CSR read/write operation, which is actually
      * claim the CLIC to find its pending highest ID, if the ID
      * is not 0, then automatically enable the mstatus.MIE, and
@@ -212,6 +230,11 @@ irq_entry:
     /* Critical section with interrupts disabled */
     DISABLE_MIE
 
+    /* All nested IRQ work claimed by JALMNXTI is done; leave ThreadX ISR context
+     * before restoring the interrupted context.
+     */
+    UPDATE_THREADX_SYSTEM_STATE -1
+
     /* Restore the necessary CSR registers */
     RESTORE_CSR_CONTEXT
     /* Restore the caller saving registers (context) */
@@ -235,4 +258,3 @@ default_intexc_handler:
     j 1b
 
     .size default_intexc_handler, . - default_intexc_handler
-

OS/ThreadX/ports_smp/nuclei/iar/context.S

@@ -65,7 +65,7 @@ _per_cpu_init_sp_fin:
     addi  t0, t0, 1
     STORE t0, 1  * REGBYTES(a0)
 
-    LOAD sp,  2  * REGBYTES(a0)      /* Read sp from _tx_thread_execute_ptr[coreid] -> tx_thread_stack_ptr */
+    LOAD sp,  2  * REGBYTES(a0)      /* Read sp from selected thread -> tx_thread_stack_ptr */
     /* Pop PC from stack and set MEPC */
     LOAD t0,  0  * REGBYTES(sp)
     csrw CSR_MEPC, t0
@@ -145,10 +145,12 @@ eclic_msip_handler:
     /* Push mstatus to stack */
     csrr t0, CSR_MSTATUS
     STORE t0,  (portRegNum - 1)  * REGBYTES(sp)
+    /* Store last pc to thread stack */
+    csrr t0, CSR_MEPC
+    STORE t0, 0(sp)
 
     /* Push additional registers */
 
-    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be saved */
 #if defined(SMP_CPU_CNT) && (SMP_CPU_CNT > 1)
     la t0, _tx_thread_current_ptr
     csrr t1, CSR_MHARTID
@@ -158,27 +160,30 @@ eclic_msip_handler:
     la t0, _tx_thread_current_ptr
 #endif
     LOAD t0, 0(t0)
-    beqz t0, _tx_thread_switch
+    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be saved */
+    beqz t0, _tx_thread_save_switch
     /* Store sp to task stack to _tx_thread_current_ptr -> tx_thread_stack_ptr */
     STORE sp, 2 * REGBYTES(t0)
-    /* Store last pc to thread stack */
-    csrr t0, CSR_MEPC
-    STORE t0, 0(sp)
 
-_tx_thread_switch:
+_tx_thread_save_switch:
     jal PortThreadSwitch
 
-    /* Switch task context to _tx_thread_execute_ptr[coreid] */
+    /* PortThreadSwitch has claimed and published the selected thread in
+       _tx_thread_current_ptr[coreid].  Restore that exact thread instead of
+       reloading _tx_thread_execute_ptr, which may be changed by another hart. */
 #if defined(SMP_CPU_CNT) && (SMP_CPU_CNT > 1)
-    la t0, _tx_thread_execute_ptr
+    la t0, _tx_thread_current_ptr
     csrr t1, CSR_MHARTID
     slli t1, t1, LOG_REGBYTES
     add t0, t0, t1
 #else
-    la t0, _tx_thread_execute_ptr
+    la t0, _tx_thread_current_ptr
 #endif
     LOAD t0, 0(t0)
+    /* If _tx_thread_current_ptr[coreid] is null, no sp need to be restored */
+    beqz t0, _tx_thread_restore_switch
     LOAD sp, 2 * REGBYTES(t0)
+_tx_thread_restore_switch:
 
     /* Pop PC from stack and set MEPC */
     LOAD t0,  0  * REGBYTES(sp)

OS/ThreadX/ports_smp/nuclei/iar/interrupt.S

@@ -5,6 +5,19 @@ DISABLE_MIE MACRO
   csrci CSR_MSTATUS, MSTATUS_MIE
   ENDM
 
+/* Update ThreadX SMP per-core ISR nesting state.
+ * This macro must be called after SAVE_CONTEXT because it clobbers t0-t2.
+ */
+UPDATE_THREADX_SYSTEM_STATE MACRO delta
+    la t0, _tx_thread_system_state
+    csrr t1, CSR_MHARTID
+    slli t1, t1, LOG_REGBYTES
+    add t0, t0, t1
+    LOAD t2, 0(t0)
+    addi t2, t2, delta
+    STORE t2, 0(t0)
+  ENDM
+
 SAVE_CONTEXT MACRO
 #if defined(ECLIC_HW_CTX_AUTO) && defined(CFG_HAS_ECLICV2)
 #else
@@ -97,6 +110,7 @@ RESTORE_CSR_CONTEXT MACRO
     PUBLIC exc_entry, irq_entry, default_intexc_handler
     PUBLIC Undef_Handler
     EXTERN core_exception_handler
+    EXTERN _tx_thread_system_state
     SECTION `.text`:CODE:NOROOT(2)
     CODE
 
@@ -162,6 +176,11 @@ irq_entry:
     /* Save the necessary CSR registers */
     SAVE_CSR_CONTEXT
 
+    /* Mark this core as being in a normal IRQ handler.
+     * eclic_msip_handler is the scheduler SWI path and is handled separately.
+     */
+    UPDATE_THREADX_SYSTEM_STATE 1
+
     /* This special CSR read/write operation, which is actually
      * claim the CLIC to find its pending highest ID, if the ID
      * is not 0, then automatically enable the mstatus.MIE, and
@@ -172,6 +191,11 @@ irq_entry:
     /* Critical section with interrupts disabled */
     DISABLE_MIE
 
+    /* All nested IRQ work claimed by JALMNXTI is done; leave ThreadX ISR context
+     * before restoring the interrupted context.
+     */
+    UPDATE_THREADX_SYSTEM_STATE -1
+
     /* Restore the necessary CSR registers */
     RESTORE_CSR_CONTEXT
     /* Restore the caller saving registers (context) */

OS/ThreadX/ports_smp/nuclei/port.c

@@ -93,11 +93,27 @@ TX_THREAD* _tx_find_ready_thread(UINT set_current)
 // Task Switch code called in eclic_msip_handler
 void PortThreadSwitch(void)
 {
+    TX_THREAD *rdy_thread;
+    UINT coreid = _tx_thread_smp_core_get();
+
+    if (_tx_thread_preempt_disable != 0) {
+        if (_tx_thread_smp_protection.tx_thread_smp_protect_core == coreid) {
+            SysTimer_ClearSWIRQ();
+        }
+        __RWMB();
+        return;
+    }
+
 #ifdef TX_ENABLE_EXECUTION_CHANGE_NOTIFY
     _tx_execution_thread_exit();
 #endif
-    TX_THREAD *rdy_thread;
-    UINT coreid = _tx_thread_smp_core_get();
+
+    /* Acknowledge the interrupt that brought us here before scheduling.
+       Any SWI raised after this point represents a new scheduling request
+       and must remain pending for the restored thread. */
+    SysTimer_ClearSWIRQ();
+    __RWMB();
+
     /*
      * Magic idle task emulation for threadx
      * ThreadX don't have idle task, so _tx_thread_execute_ptr could be NULL
@@ -131,13 +147,9 @@ void PortThreadSwitch(void)
         rv_csr_t msubm = __RV_CSR_READ(CSR_MSUBM);
         __enable_irq();
         __RWMB();
-        /*
-         * Do not touch the local rdy_thread result here.
-         * After switching to the interrupt stack, let the scheduler helper
-         * finish updating the execute pointer for this core, then reload the
-         * chosen thread after the task stack is restored.
-         */
-        _tx_find_ready_thread(TX_FALSE);
+        /* Claim the selected thread while on the interrupt stack.  Publish it
+           as current only after switching back to the task stack below. */
+        _tx_find_ready_thread(TX_TRUE);
         /* disable interrupt to avoid interrupt nesting since new task handle found */
         __disable_irq();
         __RWMB();
@@ -152,19 +164,13 @@ void PortThreadSwitch(void)
             ECLIC_SetLevelIRQ(SysTimer_IRQn, KERNEL_INTERRUPT_PRIORITY);
             __RWMB();
         }
-        __RWMB();
-        /* Now the task stack is restored, so it is safe to reload rdy_thread. */
-        rdy_thread = _tx_thread_execute_ptr[coreid];
     } else {
-        rdy_thread = _tx_find_ready_thread(TX_FALSE);
+        _tx_find_ready_thread(TX_TRUE);
     }
-
+    rdy_thread = _tx_thread_current_ptr[coreid];
     /* Clear the execution control flag.  */
     rdy_thread -> tx_thread_smp_core_control = 0;
-    _tx_thread_current_ptr[coreid] = rdy_thread;
-    _tx_thread_current_ptr[coreid] -> tx_thread_run_count ++;
-    /* Clear Software IRQ, A MUST */
-    SysTimer_ClearSWIRQ();
+    rdy_thread -> tx_thread_run_count ++;
     __RWMB();
 }