Выход из handler-mode в cortex-m3 Опции

[Omnicake]

Здравствуйте, столкнулся с проблемой. Сделал обработку прерывания на системный таймер Systick, во время обработки прерывания производится изменения битов и в конце грузятся значения регистров LR и SP для другой подзадачи (тем самым реализовано переключение задач), однако при выходе на задачу процессор почему-то остается в handler-mode - то есть по сути не выходит из прерывания. Соответственно на последующие срабатывания прерывания от таймера процессор никак не реагирует. Прочитал, что для корректного переключения режимов, надо в регистр LR заносить специальное значение EXC_RETURN, однако как это сделать если в LR мне одновременно нужно занести и значение для переключения на задачу? Как быть?

[Omnicake]

Инициализация таймера

Код

    AREA HEAP, CODE, READONLY
    
systick        PROC
            EXPORT systick
            MOV R0,#0x03e8; 0x03e8 = 1000 - количество тактов до прерывания
            MOV    R1,#0xe000e000
            STR    R0,[r1,#0x14]
            STR    R0,[r1,#0x18]
            MOVS R0,#0x07
            STR    R0,[r1,#0x10]
            ENDP        
            BX LR
            END

Обработчик вызывается из main.c таким образом

Код

void SysTick_Handler()
{
scheduler();
}

В scheduler() выполняются следующие действия:

Код

    LDR    r0,    =TaskPointer
    LDR     r0, [r0]
    LDR     r0, [r0]
    MOV     r1,    r0
        LDR sp, [r1,#24]
    LDR lr, [r1,#16]
    BX  LR

Где TaskPointer - это массив в котором содержится определенная информация о задачах, которые нужно переключать, соответственно [r1,#24] - адрес указателя стэка для задачи 1, [r1,#16] - значение pc регистра для задачи 1.
После BX LR прграмма выходит на зацикленную задачу task1.c

Код

int task1(des1* ddd)
{
int i, j, x, y;
ddd->temp = 0x12345678;
while(1)
  for( i = 0; i < 30000; i++ )
      for( j = 0; j < 30000; j++ )
    {
     x = 1;
     y = x;
     x = y;
    }
}

И на ней и остается, то есть при попытке поставить breakpoint на моменте до входа в scheduler() программа не останавливается а продолжает выполнять task1.c