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