arm - タスク切り替えがうまくいかない

Question

.macro      SAVE_CONTEXT      SWI_F
    ldr     sp,=current_p               /* switch to pcb */
    ldr     sp,[sp]                     /* get pcb ptr */
    add     sp,sp,#68                   /* point to top of stack */
.if (\SWI_F == 0)                       /* if it is not swi macro */
    sub     lr,lr,#0x04                 /* return addr */
.endif                                  /* lr -= 4 if not swi */
    stmdb   sp!,{lr}
    stmdb   sp!,{r0-r12,lr}             /* prepare to return */ 
    mrs     r1,spsr                     /* r1 <- spsr_usr */
    mrs     r2,cpsr                     /* r2 <- cpsr */
    push    {r1}                        /* save spsr_usr */
    msr     cpsr_c,#SYS_MODE|NO_INTR    /* switch to sys mode */
    mov     r3,sp                       /* r3 <- sp_usr */
    msr     cpsr,r2                     /* switch back */
    push    {r3}                        /* save sp_usr */
    mov     r0,lr                       /* r0 <-- lr */
    msr     cpsr_c,#SVC_MODE|NO_INTR    /* switch back to SVC mode */
    ldr     sp,=KERNEL_STACK            /* switch to SVC stack */
.endm

.macro      SET_ISR_PROC     ISR_PROC   /* */
    ldr     lr,=__restart               /* set return address */
    ldr     pc,=\ISR_PROC               /* call ISR */
.endm

__restart:                              /* restore context */
    ldr     sp,=current_p               /* get pcb */
    ldr     sp,[sp]                     /* adjust back to pcb */
re_restart:                             /* ret from kernel */
    pop     {r3}                        /* r3 <- sp_usr */
    mrs     r2,cpsr                     /* r2 <- cpsr */
    msr     cpsr_c,SYS_MODE|NO_INTR     /* switch to sys mode*/
    mov     sp,r3                       /* sp_usr <-- r3 */
    msr     cpsr,r2                     /* switch back */
    pop     {r1}                        /* r1 <- spsr_usr */
    msr     spsr,r1                     /* restore spsr */
    ldmia   sp!,{r0-r12,lr,pc}^         /* intr ret */

_do_irq:
    SAVE_CONTEXT    0                   /* not swi */
    SET_ISR_PROC    do_irq

このタスク切り替えコードは、s3c2440 ボードでタスクが 1 つしかない場合にうまく機能しますが、2 つ以上のタスクが作成されると、タスク 0 がタスク 1 コードを実行するか、タスク 1 がタスク 0 コードを実行します。理由はわかりません。必要 ！