Sorry, auch wenn das jetzt ein wenig hart klingen mag, aber dein ganzer Ansatz (einfach mal in der ISR die Rücksprungadresse zu ändern) ist totaler Murks. Es ist völlig illusorisch zu denken, auf die Art ein Multitasking einfach realisieren zu können. Das führt unweigerlich zu Chaos und Absturz. Wenn du preemptives Multitasking haben willst, dann führt absolut kein Weg an einem kompletten Kontext-Wechsel vorbei.

Deine eigentliche erste Aufgabe ist es also nicht, zu überlegen "wie komme ich am Ende der ISR zu einer anderen Funktion" (das ist total irrelevant, denn das passiert beim echten Kontext-Wechsel ganz von alleine), sondern zu überlegen "was gehört eigentlich alles zum Kontext".