При входе в важные участки устанавливайте признак критичной секции и "фиксируйте-дублируйте" все что Вам нужно, или запоминайте текущую позицию алгоритма, а при выходе из них снимайте этот признак.
(«Телесистемы»: Конференция «Микроконтроллеры и их применение»)