Warum dann nicht einfach mit -mcall-prologues compilieren und die ISRs naked attributieren? Das hält foo() kurz und sorgt auch dafür, dass die Register sauber bleiben (und man bleibt weitestgehend bei C).

foo() und die ISR sollten dann in einer eigenen Datei sein, da sonst alles mit -call-prologues compiliert wird was boshafterweise auch zu größeren Code führen kann, da der gcc-avr bei der Parameterübergabe in Registern schon sehr gut optimiert.