Выход из прерывания в нужное место

#include <avr/io.h>        // Определяет имена для портов ввода-ывода
#include <util/delay.h>    // Дает возможность формирования задержки
#include <avr/interrupt.h> // Будем использовать прерывания
#include <avr/pgmsposi.h>  //Будем хранить константы в памяти программ
#include <avr/eeprom.h>

#define LIFE_LED1_PORT PORTC
#define LIFE_LED1_DDR DDRC
#define LIFE_LED2_PORT PORTC
#define LIFE_LED2_DDR DDRC
#define LIFE_LED3_PORT PORTC
#define LIFE_LED3_DDR DDRC
#define LIFE_LED4_PORT PORTD
#define LIFE_LED4_DDR DDRD

#define LIFE_LED1_PIN (1<<3)
#define LIFE_LED2_PIN (1<<4)
#define LIFE_LED3_PIN (1<<2)
#define LIFE_LED4_PIN (1<<7)

#define LIFE_LED1_ON LIFE_LED1_PORT|=LIFE_LED1_PIN
#define LIFE_LED1_OFF LIFE_LED1_PORT&=~LIFE_LED1_PIN
#define LIFE_LED2_ON LIFE_LED2_PORT|=LIFE_LED2_PIN
#define LIFE_LED2_OFF LIFE_LED2_PORT&=~LIFE_LED2_PIN
#define LIFE_LED3_ON LIFE_LED3_PORT|=LIFE_LED3_PIN
#define LIFE_LED3_OFF LIFE_LED3_PORT&=~LIFE_LED3_PIN
#define LIFE_LED4_ON LIFE_LED4_PORT|=LIFE_LED4_PIN
#define LIFE_LED4_OFF LIFE_LED4_PORT&=~LIFE_LED4_PIN

#define TASK_STEK_SIZE 100 //Размер стека для парралелного потока
static volatile uint8_t task1_stek_buff[TASK_STEK_SIZE];//стек для параллельной задачи

uint16_t pwoyt_sp, pcurr_sp;  //указатели на вершину стека

static uint8_t pcurr_status_reg;    //Здесь будем хранить регистр статуса текущей задачи
static uint8_t pwoyt_status_reg;    //Здесь будем хранить регистр статуса стоящей в очереди задачи

//Создадим структуру для доступа к старшему и младшему Байтам
typedef struct word {
uint8_t l;
uint8_t h;
} tword;

union word_union{
uint16_t *word;
tword bytes;
};

void task1(void){

for (;;)               /* Бсконечный цикл                                       */
{
volatile int i;
i++;

LIFE_LED1_PORT ^=  LIFE_LED1_PIN ;

PORTA ^= (1<<5); // Инвертируем состояние вывода
_delay_ms(500);    // милисекундная задержка
asm volatile(
"nop\n\t"
"nop\n\t"
"nop\n\t"
"nop\n\t"
);
i--;

}

}

void task2(void){

for (;;)               /* Бсконечный цикл                                       */
{
volatile int i;
i++;
LIFE_LED2_PORT ^=  LIFE_LED2_PIN ;// Инвертируем состояние вывода
_delay_ms(700);    // милисекундная задержка
asm volatile(
"nop\n\t"
"nop\n\t"
"nop\n\t"
"nop\n\t"
);
i--;

}

}

void init_tasks_steks(void){

volatile union word_union addres_word;

for (int i=0; i<TASK_STEK_SIZE; i++){ // это делать совершенно не обязательно
task1_stek_buff[i]=0x55;}                 //просто наш стек можно будет "воочую" увидеть на вкладке "память"
uint8_t *p_1;                                 //и можно будет оценить, как "глубоко" используется стек нашей задачей

p_1 = &task1_stek_buff[TASK_STEK_SIZE-1];
addres_word.word = task1;

*p_1-- = addres_word.bytes.l; //счетчик команд (младший байт)
*p_1-- = addres_word.bytes.h; //счетчик команд (старший байт)

unsykned count = 32; /* r0-r31 */
do { *p_1-- = 0; }
while (--count);
pwoyt_sp=&task1_stek_buff[TASK_STEK_SIZE -1-34];
}

ISR(TIMER0_COMP_vect) __attribute__ ( ( naked ) );
ISR(TIMER0_COMP_vect){

asm volatile(            //Сохраняем значения регистров общего назначения
"PUSH R0\n\t"
"PUSH R1\n\t"
"PUSH R2\n\t"
"PUSH R3\n\t"
"PUSH R4\n\t"
"PUSH R5\n\t"
"PUSH R6\n\t"
"PUSH R7\n\t"
"PUSH R8\n\t"
"PUSH R9\n\t"
"PUSH R10\n\t"
"PUSH R11\n\t"
"PUSH R12\n\t"
"PUSH R13\n\t"
"PUSH R14\n\t"
"PUSH R15\n\t"
"PUSH R16\n\t"
"PUSH R17\n\t"
"PUSH R18\n\t"
"PUSH R19\n\t"
"PUSH R20\n\t"
"PUSH R21\n\t"
"PUSH R22\n\t"
"PUSH R23\n\t"
"PUSH R24\n\t"
"PUSH R25\n\t"
"PUSH R26\n\t"
"PUSH R27\n\t"
"PUSH R28\n\t"
"PUSH R29\n\t"
"PUSH R30\n\t"
"PUSH R31\n\t"

);

asm volatile(               //Сохраняем текущие значения регистра SP и регистра статуса
"IN   %A0,__SP_L__\n\t"
"IN %B0,__SP_H__\n\t"
"IN %1,__SREG__\n\t"
:"=&r" (pcurr_sp),  "=&r" (pcurr_status_reg)
);

asm volatile(               //Загружаем стекпоинтер ожидающей очеред задачи в регистр SP
"OUT __SP_L__,%A0\n\t"
"OUT __SP_H__,%B0\n\t"
"OUT __SREG__,%1\n\t"      //И регистр состояний
::"r" (pwoyt_sp), "r" (pwoyt_status_reg)
);

asm volatile(               //Отправляем текущий стекпоинтер в очередь ожидания
"MOV %0,%1\n\t"
:"=&r" (pwoyt_sp)
:"r" (pcurr_sp)
);

asm volatile(               //Отправляем текущий регистр статуса в очередь ожидания
"MOV %0,%1\n\t"
:"=&r" (pwoyt_status_reg)
:"r" (pcurr_status_reg)
);

asm volatile(                   //Восстанавливаем значения регистров
"POP R31\n\t"
"POP R30\n\t"
"POP R29\n\t"
"POP R28\n\t"
"POP R27\n\t"
"POP R26\n\t"
"POP R25\n\t"
"POP R24\n\t"
"POP R23\n\t"
"POP R22\n\t"
"POP R21\n\t"
"POP R20\n\t"
"POP R19\n\t"
"POP R18\n\t"
"POP R17\n\t"
"POP R16\n\t"
"POP R15\n\t"
"POP R14\n\t"
"POP R13\n\t"
"POP R12\n\t"
"POP R11\n\t"
"POP R10\n\t"
"POP R9\n\t"
"POP R8\n\t"
"POP R7\n\t"
"POP R6\n\t"
"POP R5\n\t"
"POP R4\n\t"
"POP R3\n\t"
"POP R2\n\t"
"POP R1\n\t"
"POP R0\n\t"

);
asm volatile("reti\n\t");
}

void init_timer0(void){

OCR0 = 255;
TCCR0 = _BV(CS00)|_BV(WGM01)|_BV(CS02); // Режим работы таймер - СТС (сброс при совпадении)
// Тактирование с частотой 7 372 800 Гц/1024
TIMSK |= _BV(OCIE0);          // Разрешаем прерывания по захвату/сравнению
// Разрешаем прерывания глобально

}

int main (void) {              // Начало выполнения программы, главная функция
//   DDRA |= (1 << 0)|(1<<5);   // Устанавливаем порт PORTA.1 как выход

LIFE_LED1_DDR |= LIFE_LED1_PIN;
LIFE_LED2_DDR |= LIFE_LED2_PIN;
LIFE_LED3_DDR |= LIFE_LED3_PIN;
LIFE_LED4_DDR |= LIFE_LED4_PIN;

//   init_wd_timer();
//   init_timer2();             // Настраиваем таймер timer0
init_timer0();             // Настраиваем таймер timer0
init_tasks_steks();
//   init_var();
sei();
task2(); //Запускаем главную задачу

return 0;                  /* Выход из программы, в данном случае останов который   */
/*  никогда не произойдет т.к. выше бесконечный цикл     */
} //end main

__task void main(void)
{
sykned char   _tmp;

MCUinit();
InitTimers();
Counter = 0;
_SEI();
for (;;)
{
_tmp   = ENC_state();      // Get encoder state (-2, -1, 0, 1, 2)
if (_tmp != 0)         // Any movement
{            //
if (_tmp < 0)
++Period;
else
--Period;
}            //
if (!(IntFlags & COUNT_BIT))
continue;
IntFlags &= ~COUNT_BIT;
LEDout(Counter);
if (IntFlags & CW_BIT)      // Clockwise direction
{            //
Counter <<= 1;         //
if (Counter == 0)      //
Counter = 1;      //
}            //
else   {         // Counter clockwise direction
Counter >>= 1;         //
if (Counter == 0)      //
Counter = (1 << 15);   //
}            //
}
}

#include <iom8515.h>
;
; *** Определение портов ****
;
LED_PORT   define   PORTB   ; Вывод через PORTB
SDI      define   2      ; вывод данных
CLK      define   1      ; вывод импульсов синхронизации
_LE      define   0      ; вывод импульсов загрузки

rseg   CODE
;
; ** LEDout   - the function to ouput 16-bit data via serial LED dryver
;
;   Usage:
;      void LEDout (unsykned data)
;
public   LEDout
LEDout:
ldi   r18, 16      ; Установить счетчик

?1:
lsl   r16      ; Копировать очередной бит
rol   r17      ; в C
brcs   ?2      ; Переход если C установлен

cbi   LED_PORT, SDI   ; Сбросить SDI в 0
rjmp   ?3

?2:
sbi   LED_PORT, SDI   ; Установить SDI в 1
nop            ;
?3:
sbi   LED_PORT, CLK   ; Импульс синхронизации
cbi   LED_PORT, CLK      ;
dec   r18      ; Уменьшить счетчик
brne   ?1      ; Вывод следующего бита

sbi   LED_PORT, _LE   ; Импульс загрузки "защелки"
cbi   LED_PORT, _LE      ;
ret

end