U32 NVIC_INT_CTRL  = 0xE000ED04;            // Interrupt control state register
U32 NVIC_PENDSVSET = 0x10000000;            // Value to trigger PendSV exception
U32 INT_EXIT       = 0xFFFFFFFC;

void SetEnvironment(OS_STK *pstk)   {
    __asm volatile
    (   
       " SUB    R0,#28 \n"      // R0 = R0 - 28
       " MSR    PSP,R0 \n"      // PSP = R0 ( Move the contents of a general-purpose register to a special register. )
       " BX      LR  \n"        // BX is branch indirect (register)
    );
}

/**
******************************************************************************
* @brief      Do ready work to Switch Context for task change
* @param[in]  None   
* @param[out] None 
* @retval     None.      
*
* @par Description
* @details    This function is called to Do ready work to
*              Switch Context for task change
******************************************************************************
*/
void SwitchContext(void)    {
  __asm volatile
  (
      " LDR     R3,=NVIC_INT_CTRL  \n"
     " LDR     R3,[R3]            \n"
     " LDR     R2,=NVIC_PENDSVSET \n"
     " LDR     R1,[R2]            \n"
      " STR     R1, [R3]           \n"
      " BX      LR               \n"   
  );
}

void PendSV_Handler(void)
{
////////debug block /////////////////////////
  __asm volatile
(
    " LDR    R3,=TCBRunning \n"
    " LDR    R1,[R3]        \n"    // R1 == running tcb
    " LDR    R2,=TCBNext    \n"
    " LDR    R2,[R2]        \n"    // R2 == next tcb
   
    " CMP    R1,R2          \n"
    " BEQ    exitPendSV     \n"             
   
    " MRS    R0, PSP        \n"    // Get PSP point (can not use PUSH,in ISR,SP is MSP )
    " STMDB  R0!,{R4-R11}   \n"    // Store r4-r11,r0 -= regCnt * 4,r0 is new stack
                                   // top point (addr h->l r11,r10,...,r5,r4)
    " STR    R0,[R1]        \n"    // Save orig PSP
     
    " STR    R2, [R3]       \n"    // TCBRunning  = TCBNext;
    " LDR    R0, [R2]       \n"    // Get SP of task that be switch into.
    " LDMIA  R0!,{R4-R11}   \n"    // POP {R4-R11},R0 += regCnt * 4
    " MSR    PSP, R0        \n"    // Mov new stack point to PSP
   

    " exitPendSV:           \n"
   " LDR    R3,=OSSchedLock\n"
   " MOVS   R0, #0x0       \n"
   " STRB   R0, [R3]       \n"
    " ORR    LR,LR,#0x04    \n"    // Ensure exception return uses process stack
    " BX     LR             \n"    // Exit interrupt
  );
}

//*----------------------------------------------------------------------------
//*         PendSV handler
//*----------------------------------------------------------------------------
FUNC(   sys_pendsv_handler   )
   mrs               r3, PSP                                                        // Recover PSP into R12
   stmia             REG_CTASK, {r3-r9}                                             // Push non-stack registers
   ldr               REG_CTASK, [REG_MTASK, #tsk_renext]
   ldmia             REG_CTASK, {r3-r9}                                             // Restore non-stacked registers
   msr               PSP, r3                                                        // Set PSP to R12
   bx                lr                                                             // Return back to Thread
ENDF(   sys_pendsv_handler   )

ALWAYS_INLINE
void port_context_switch(struct port_context *from, struct port_context *to)
{
   asm volatile("\n"
         "stmia   %0,{r0-r15}\n"
         "ldmia   %1,{r0-r15}"
#if !defined(ARM_STM_PC_OFFSET) || ARM_STM_PC_OFFSET==12
         "\n nop"
#endif
         : /* no outputs */
         : "r"(from), "r"(to)
         : "memory");
}      /* -----  end of function port_context_switch  ----- */

NEVER_INLINE
__attribute__((naked))
void port_context_switch(struct port_context *from, struct port_context *to)
{
   asm volatile("\n"
         "push   {r4-r11,lr}\n"
         "str   sp,[%0]\n"
         "ldr   sp,[%1]\n"
         "pop   {r4-r11,pc}"
         : /* no outputs */
         : "r"(&from->stack), "r"(&to->stack)
         : "memory");
}      /* -----  end of function port_context_switch  ----- */

void task_yeld()
{
   port_interrupt_status interrupt_status = port_interrupt_disable();

   struct task *old_task = task_current();

   task_list.tail = list_head(&task_list);

   struct task *new_task = task_current();

   port_context_switch(&old_task->context, &new_task->context);

   port_interrupt_restore(interrupt_status);
}      /* -----  end of function task_yeld  ----- */

00000264 <task_yeld>:
264:   e10f2000    mrs   r2, CPSR
268:   e38230c0    orr   r3, r2, #192   ; 0xc0
26c:   e121f003    msr   CPSR_c, r3
270:   e59f1024    ldr   r1, [pc, #36]   ; 29c <task_yeld+0x38>
274:   e5913000    ldr   r3, [r1]
278:   e5933000    ldr   r3, [r3]
27c:   e5813000    str   r3, [r1]
280:   e2831004    add   r1, r3, #4
284:   e5933000    ldr   r3, [r3]
288:   e2833004    add   r3, r3, #4
28c:   e881ffff    stm   r1, {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, sl, fp, ip, sp, lr, pc}
290:   e893ffff    ldm   r3, {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, sl, fp, ip, sp, lr, pc}
294:   e121f002    msr   CPSR_c, r2
298:   e12fff1e    bx   lr
29c:   30000094    .word   0x30000094