Ошибка при отладке в AVR Studio маленького макроса Опции

[_Алекс]

Была задача получить текущий адрес выполнения программы – программный счетчик PC.
Среда разработки IAR
Запомнить его, сделал вот так, результат, при отладке в AVR STUDIO выдает ошибку с переполнение стека после нескольких прогонах программы, интересно что при вызове rcall L1, стек увеличивается на 2 байта, при pop, pop уменьшается на два, вобщем кажется все правильно.

Макрос взятия адреса и функция в макросе, которая по идеи должна сохранять PC, но просто сделана как тестовая.

#define OutSaveContekst\
{\
asm ("rcall L1\n" "L1: pop r16\n" "pop r17\n");\
SaveContext();\
\
}

Функция SaveContext (по идеи сохранение контекста) ни чего полезного пока
void SaveContext(void){
static int t = 0;
t++;
};

Функция тест в которой применен макрос
void test(void){
static char i;
if (i = = 5){
OutSaveContekst;
i = i + 1;
};
i = i + 1;
};

Главная функция
void main( void )
{
while(1){
test();
}
};

[Alex ma]

Получилось вот так
unsigned int OutSaveContekst(void){
return (*(unsigned int *)(SP + 2));
};

Возвращает адрес во Flash на оператор после вызова функции

18: it = OutSaveContekst();
+0000002B: D005 RCALL PC+0x0006 Relative call subroutine
+0000002C: EDE4 LDI R30,0xD4 Load immediate
+0000002D: E0F0 LDI R31,0x00 Load immediate
+0000002E: 8300 STD Z+0,R16 Store indirect with displacement
+0000002F: 8311 STD Z+1,R17 Store indirect with displacement
Возвращает 0х002С

Корректно будет если 0х002С + 4, т.е
unsigned int OutSaveContekst(void){
return ((*(unsigned int *)(SP + 2)) + 4);
};
Тогда возвращает 0х0030
18: it = OutSaveContekst();
+0000002B: D005 RCALL PC+0x0006 Relative call subroutine
+0000002C: EDE4 LDI R30,0xD4 Load immediate
+0000002D: E0F0 LDI R31,0x00 Load immediate
+0000002E: 8300 STD Z+0,R16 Store indirect with displacement
+0000002F: 8311 STD Z+1,R17 Store indirect with displacement
@00000030: _..X_RSTACK_BASE
19: };
+00000030: 9508 RET Subroutine return

Будет так работать, указывать на следующий оператор во флеш после вызова функции, затем можно выйти return, а потом вернуться icall, Будет работать?

PS
_Alex на работе,
Alex ma дома.