Klingt nach Timing-Problemen - ich sehe auch nicht, wo du Interrupts vor dem Betreten des kritischen Abschnittes deaktivierst bzw. danach wiederherstellst.

mfG
Markus