Добрый день, понадобилось портировать свой проект на другую плату, а там половина мелкой периферии(кнопки термодатчики, дисплей) сидит все на I2C, и все должно гарантированно работать, после замыкания размыкания линии.

Так вот из за этого решил сделать для диспетчер для I2C, который будет запускать все процессы периферии касательно I2C и обрабатывать аварии. Так как периферии много и вложений функций больше 3-4 решил сделать во время ожидания флагов I2C выход с сохранением адреса возврата.

Вначале функции поставил переход на адрес ожидания флагов, а само ожидание сделал так:


Для одной функции всего работает, но вот как появились вложения, то там сразу все повисло из за того что миную вход в функцию,а выход есть.Пока думаю в сторону что бы выход происходит все время в диспетчер,а он там сам разгребался.
В общем, какое то мутное представление всего, как это выглядит правильно ?