Napadá mě akorát si dát celou hlavní smyčku jako funkci. Tímpádem po initu hardware, se ti zavolá funkce a naplní stack. Při ukončení přerušení, bych ukazatel Stacku vynuloval. Na PIC jsem to takto párkrát použil, u AVR by podobný princip měl jít taky.
cpu_init();
while (1)
{
sw_init(); //Tohle se pak automaticky vykoná po ukončení přerušení
hlavni_smycka();
}
//-----------
void hlavni_smycka (void)
{
while(1)
{
//Hlavní programová smyčka
}
}