ARM, RISC-V контроллеры

Задумал извращение - ОС приклеивать желания нет, а тяжелые функции из стороннего кода хочется на время откладывать. Решение напоминает ОСь, только вместо других задач идет фон - прервать таймером, сохранить регистры, восстановить регистры фона и выпрыгнуть в фон. Проц STM32F100. Вот такой быдлокод: #define OS_TT_STACK_SIZE 1500 //размер стека для прерываемой функции __root U8 OS_TT_Stack[ OS_TT_STACK_SIZE ]; U8 *OS_TT_Stack_Pointer;//указатель - куда надо поставить для возобновления задачи U8 OS_TT_Suspended;//флаг того что функция приостановлена U8 *OS_TT_Original_SP; U8 WantRet = false;//попросить функцию завершиться void TESTFUNC(void) { U8 Counter1 = 0, Counter2 = 0; while(1) { if( Counter1++ & 1 ) LED_Red(1); else LED_Red(0); if( Counter2++ & 2 ) LED_Green(1); else LED_Green(0); if( WantRet ) break; } } void Setup_Tmr( void ) { #define OS_TT_TIM TIM7 RCC -> APB1ENR |= RCC_APB1ENR_TIM7EN; //получаем прерывание через 1мс OS_TT_TIM -> CR1 = 0; OS_TT_TIM -> CNT = 0; OS_TT_TIM -> CR2 = 0; OS_TT_TIM -> PSC = MAINCLOCK / 2400 - 1;//получим такт 0.1мс OS_TT_TIM -> ARR = 1000 / 100;//до куда надо насчитать OS_TT_TIM -> EGR = TIM_EGR_UG; OS_TT_TIM -> SR = 0; OS_TT_TIM -> DIER = TIM_DIER_UIE; IRQ_Set_Priority( TIM7_IRQn, 7 ); IRQ_Enable( TIM7_IRQn ); } //чистим прерывание и пускаем по новой таймер #define RunTmr() do{ OS_TT_TIM -> CNT = 0; OS_TT_TIM -> SR = 0; OS_TT_TIM -> CR1 = TIM_CR1_CEN; }while(0) __irq void TIM7_IRQ_Handler(void) { //имеем регистры сохраненные в стек //********************** //суем дополнительные нужные регистры asm("PUSH {R4-R11}"); asm("MRS R7, BASEPRI");//r12 crashed asm("PUSH {R4-R9}");//да можно только R7 но пока так //********************** //запоминаем SP задачи - то что насохряняло прерывание и мы поверху насовали OS_TT_Stack_Pointer = (U8*)( __get_SP() ); //********************** //поднимаем назад фон __set_SP( (U32)OS_TT_Original_SP ); //выкл таймер OS_TT_TIM -> CR1 = 0; OS_TT_TIM -> SR = 0; //ставим флаг что функция прервана OS_TT_Suspended = true; //ВОЗВРАТ ПОЙДЕТ В ФОН!!!! А НЕ В ФУНКЦИЮ //точка А <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< } //запускатель функции void OS_TT_Run_Function( void ) { //сохраняем регистры фона аналогично тому как это делает прерывание asm("MRS R12,APSR"); asm("PUSH {R12}");//APSR //адрес возврата задаем на SW_LABELF asm("ADR R12,SW_LABELF"); asm("PUSH {R12}");//PC asm("PUSH {R0-R3,R12,LR}"); //запоминаем указатель стека OS_TT_Original_SP = (U8*)__get_SP(); DI(); if( !OS_TT_Suspended ) { RunTmr(); __set_SP( (U32)&OS_TT_Stack[ OS_TT_STACK_SIZE - 1 ] );//указатель на начало выделенного стека EI(); TESTFUNC();//вызываемая функция OS_TT_Suspended = false; } else { RunTmr(); EI(); TesterTTRestore();//восстанавливаем все и гоним задасу по новой } asm("SW_LABELF:");//куда вернуться после прерывания функции } void TesterTTRestore( void ) { //восстанавливаем SP задачи - откуда поднимать регистры __set_SP( (U32)OS_TT_Stack_Pointer ); //********************** //восстанавливаем дополнительные регистры asm("POP {R4-R9}"); asm("MSR BASEPRI,R7"); asm("POP {R4-R11}"); //********************** //********************** //ОСНОВНЫЕ РЕГИСТРЫ asm("POP {R0-R3,R12,LR}");//те что сохраняет прерывание asm("POP {R12}");//PC пустое чтение чтобы пропустить asm("POP {R12}");//APSR asm("MSR APSR, R12"); //все стек мы выровняли //********************** //чиним опять R12 asm("SUB SP,SP,#16"); asm("LDR R12, [SP], #8"); //********************** //переходим на задачу asm("LDR PC, [SP], #8");//+8 } Не обращайте внимания на неоптимальность или даже тупость некоторых вещей, это еще успеется после того как все заработает. Вроде все функционирует, сохраняет и восстанавливает, ничего не рушится. НО! РАБОТАЕТ ТОЛЬКО ПОШАГОВО. Как только запустить - в точке А при возврате из прерывания вываливается в fault с флагом INVSTATE. Т.е. якобы чаще всего причина - возврат на нечетный адрес. Но в стеке он сохранен отлично четный. Где я туплю? Почему пошагово пашет а при запуске сразу падает?