__asm void prvStartFirstTask( void )
{
    PRESERVE8

    /* Use the NVIC offset register to locate the stack. */
    ldr r0, =0xE000ED08
    ldr r0, [r0]
    ldr r0, [r0]
    /* Set the msp back to the start of the stack. */
    msr msp, r0
    /* Clear the bit that indicates the FPU is in use in case the FPU was used
    before the scheduler was started - which would otherwise result in the
    unnecessary leaving of space in the SVC stack for lazy saving of FPU
    registers. */
    mov r0, #0
    msr control, r0
    /* Globally enable interrupts. */
    cpsie i
    cpsie f
    dsb
    isb
    /* Call SVC to start the first task. */
    svc 0
    nop
    nop
}

__asm void vPortSVCHandler( void )
{
    PRESERVE8

    /* Get the location of the current TCB. */
    ldr    r3, =pxCurrentTCB
    ldr r1, [r3]
    ldr r0, [r1]
    /* Pop the core registers. */
    ldmia r0!, {r4-r11, r14}
    msr psp, r0
    isb
    mov r0, #0
    msr    basepri, r0
    bx r14
}

__asm void prvStartFirstTask( void )
{
    PRESERVE8

    /* Use the NVIC offset register to locate the stack. */
    ldr r0, =0xE000ED08
    ldr r0, [r0]
    ldr r0, [r0]
    /* Set the msp back to the start of the stack. */
    msr msp, r0
    /* Clear the bit that indicates the FPU is in use in case the FPU was used
    before the scheduler was started - which would otherwise result in the
    unnecessary leaving of space in the SVC stack for lazy saving of FPU
    registers. */
    mov r0, #0
    msr control, r0
    /* Globally enable interrupts. */
    cpsie i
    cpsie f
    dsb
    isb

        // Финт ушами

        /* Get the location of the current TCB. */
    ldr    r3, =pxCurrentTCB
    ldr r1, [r3]
    ldr r0, [r1]
    // восстановить контекст первой задачи в нужной последовательности
    ldmia r0!, {тут список регистров R0-R12, LR, XPSR в правильном порядке}
    msr psp, r0
    isb
    mov r0, #0
    msr    basepri, r0
        // а тут восстановить PC и поехали
}

xTaskHandle Task1Handle;
xTaskHandle Task2Handle;

void Task1(void *Parameters)
{
while(1)
{
HW_LED_TOGGLE(GPIO_LED1);

vTaskDelay(1000);
}
}

void Task2(void *Parameters)
{
while(1)
{
HW_LED_TOGGLE(GPIO_LED2);

vTaskDelay(500);
}
}

int main(void)
{
HW_MCUInit();

xTaskCreate(&Task1, "Task1", 128, NULL, 1, &Task1Handle);
xTaskCreate(&Task2, "Task2", 128, NULL, 1, &Task2Handle);

vTaskStartScheduler();

return 0;
}

__asm void prvStartFirstTask(void)
{
    PRESERVE8
    
    // установка MSP
    ldr r0, =0xE000ED08
    ldr r0, [r0]
    ldr r0, [r0]
    msr msp, r0
    
    dsb
    
    // получение указателя на стековый фрейм задачи
    ldr r3, =pxCurrentTCB
    ldr r1, [r3]
    ldr r0, [r1]
    
    // последовательная загрузка контекста
    ldmia r0!, {r4-r11, r14} // загрузка R4-R11, LR (LR здесь уже, по сути, является аттавизмом)
    msr psp, r0              // установка PSP
    
    dsb
    
    // перевод CPU на использование стека PSP
    mov r0, #2
    msr control, r0
    
    dsb
    
    // извлечение XPSR из стекового фрейма
    mrs r1, psp
    add r1, r1, #28
    ldr r0, [r1]
    msr xpsr, r0
    
    // извлечение оставшихся регистров
    pop {r0-r3, r12, r14}
    
    // корректировка бита 0 регистра PC
    push {r0, r1}
    
    mrs r0, psp
    add r0, r0, #8
    ldr r1, [r0]
    orr r1, r1, #1
    str r1, [r0]
    
    pop {r0, r1}
    
    // переход на активную задачу
    pop {r15}
}

By default, Thread mode uses the MSP. To switch the stack pointer used in Thread mode to the
PSP, either:
• use the MSR instruction to set the Active stack pointer bit to 1, see MSR on page 3-164.
• perform an exception return to Thread mode with the appropriate EXC_RETURN value,
see Table 2-17 on page 2-28.

;**********************************************************************
**********************************
; START MULTITASKING
; void OSStartHighRdy(void)
;
; Note(s) : 1) This function triggers a PendSV exception (essentially, causes a context switch) to cause
; the first task to start.
;
; 2) OSStartHighRdy() MUST:
; a) Setup PendSV exception priority to lowest;
; Set initial PSP to 0, to tell context switcher this is first run;
; c) Set the main stack to end of OS_CPU_ExceptStk
; d) Set OSRunning to TRUE;
; e) Trigger PendSV exception;
; f) Enable interrupts (tasks will run with interrupts enabled).
;*******************************************************************************
*************************

THUMB
OSStartHighRdy LDR R0, =NVIC_SYSPRI14 ;Set the PendSV exception priority
LDR R1, =NVIC_PENDSV_PRI
STRB R1, [R0]
LDR R1, stkBase ;Initialize the MSP to the end of OS_CPU_ExceptStk
MSR MSP, R1
LDR R0, =OSRunning ;OSRunning = TRUE
MOVS R1, #1
STRB R1, [R0]
BL OSCtxSw
LDR R3, =OSGlobalV
LDRB R2, [R3, #OSPrioHighRdy] ;OSPrioHighRdy
STRB R2, [R3, #OSPrioCur] ;OSPrioCur = OSPrioHighRdy
LDR R2, [R3, #OSTCBHighRdy] ;OSTCBHighRdy
STR R2, [R3, #OSTCBCur] ;OSTCBCur = OSTCBHighRdy;
LDR R0, [R2, #OSTCBStkPtr] ;R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
#ifdef __ARMVFP__
LDRH R1, [R2, #OSTCBOpt] ;OSTCBHighRdy->OSTCBOpt
LSLS R1, R1, #31 - OS_TASK_OPT_SAVE_FP_BIT
ITTT MI
VLDMIAMI R0!, {S0-S31}
LDMIAMI R0!, {R1}
VMSRMI FPSCR, R1
#endif
LDMFD R0!, {R4-R11} ;Restore r4-11 from new process stack
MSR PSP, R0 ;Load PSP with new process SP
CPSIE I ;Exception return will restore remaining context
startHR_01: B startHR_01

DATA
stkBase DC32 OS_CPU_ExceptStk + OS_CPU_EXCEPT_STK_SIZE * 4

;**********************************************************************
**********************************
; HANDLE PendSV EXCEPTION
; void OS_CPU_PendSVHandler(void)
;
; Note(s) : 1) PendSV is used to cause a context switch. This is a recommended method for performing
; context switches with Cortex-M3. This is because the Cortex-M3 auto-saves half of the
; processor context on any exception, and restores same on return from exception. So only
; saving of R4-R11 is required and fixing up the stack pointers. Using the PendSV exception
; this way means that context saving and restoring is identical whether it is initiated from
; a thread or occurs due to an interrupt or exception.
;
; 2) Pseudo-code is:
; a) Get the process SP, if 0 then skip (goto d) the saving part (first context switch);
; Save remaining regs r4-r11 on process stack;
; c) Save the process SP in its TCB, OSTCBCur->OSTCBStkPtr = SP;
; d) Call OSTaskSwHook();
; e) Get current high priority, OSPrioCur = OSPrioHighRdy;
; f) Get current ready thread TCB, OSTCBCur = OSTCBHighRdy;
; g) Get new process SP from TCB, SP = OSTCBHighRdy->OSTCBStkPtr;
; h) Restore R4-R11 from new process stack;
; i) Perform exception return which will restore remaining context.
;
; 3) On entry into PendSV handler:
; a) The following have been saved on the process stack (by processor):
; xPSR, PC, LR, R12, R0-R3
; Processor mode is switched to Handler mode (from Thread mode)
; c) Stack is Main stack (switched from Process stack)
; d) OSTCBCur points to the OS_TCB of the task to suspend
; OSTCBHighRdy points to the OS_TCB of the task to resume
;
; 4) Since PendSV is set to lowest priority in the system (by OSStartHighRdy() above), we
; know that it will only be run when no other exception or interrupt is active, and
; therefore safe to assume that context being switched out was using the process stack (PSP).
;*******************************************************************************
*************************

THUMB
OS_CPU_PendSVHandler:
CPSID I ;Prevent interruption during context switch
MRS R0, PSP ;PSP is process stack pointer
LDR R3, =OSGlobalV
STMFD R0!, {R4-R11} ;Save remaining regs r4-11 on process stack
LDR R2, [R3, #OSTCBCur] ;OSTCBCur
#ifdef __ARMVFP__
LDRH R1, [R2, #OSTCBOpt] ;OSTCBCur->OSTCBOpt
LSLS R1, R1, #31 - OS_TASK_OPT_SAVE_FP_BIT
ITTT MI
VMRSMI R1, FPSCR
STRMI R1, [R0, #-4]!
VSTMDBMI R0!, {S0-S31}
STR R0, [R2, #OSTCBStkPtr] ;OSTCBCur->OSTCBStkPtr = SP;
LDR R2, [R3, #OSTCBHighRdy] ;OSTCBHighRdy
STR R2, [R3, #OSTCBCur] ;OSTCBCur = OSTCBHighRdy;
LDR R0, [R2, #OSTCBStkPtr] ;R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
LDRH R1, [R2, #OSTCBOpt] ;OSTCBHighRdy->OSTCBOpt
LSLS R1, R1, #31 - OS_TASK_OPT_SAVE_FP_BIT
ITTT MI
VLDMIAMI R0!, {S0-S31}
LDMIAMI R0!, {R1}
VMSRMI FPSCR, R1
#else
STR R0, [R2, #OSTCBStkPtr] ;OSTCBCur->OSTCBStkPtr = SP;
LDR R0, [R3, #OSTCBHighRdy] ;OSTCBHighRdy
STR R0, [R3, #OSTCBCur] ;OSTCBCur = OSTCBHighRdy;
LDR R0, [R0, #OSTCBStkPtr] ;R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
#endif
LDRB R1, [R3, #OSPrioHighRdy] ;OSPrioCur = OSPrioHighRdy;
STRB R1, [R3, #OSPrioCur]
ORR LR, LR, #4 ;Ensure exception return uses process stack
LDMFD R0!, {R4-R11} ;Restore r4-11 from new process stack
MSR PSP, R0 ;Load PSP with new process SP
CPSIE I
BX LR ;Exception return will restore remaining context

+------------+ +------------+
| xPSR | | xPSR |
+------------+ +------------+
|Return Addr | |Return Addr |
+------------+ +------------+
| LR(R14) | | LR(R14) |
+------------+ +------------+
| R12 | | R12 |
+------------+ +------------+
| R3 | | R3 |
+------------+ +------------+
| R2 | | R0 |
+------------+ +------------+
| R1 | | R1 |
+------------+ +------------+
| R0 | | R0 |
+------------+ +------------+
| R11 | | R11 |
+------------+ +------------+
| R10 | | R10 |
+------------+ +------------+
| R9 | | R9 |
+------------+ +------------+
| R8 | | R8 |
+------------+ +------------+
| R7 | | R7 |
+------------+ +------------+
| R6 | | R6 |
+------------+ +------------+
| R5 | | R5 |
+------------+ +------------+
| R4 | | R4 |
+------------+ +------------+
(a) | FPSCR |
+------------+
| S31 |
+------------+
.
.
.
+------------+
| S1 |
+------------+
| S0 |
+------------+