Wenn du nicht auf den Status prüfst, sondern über nen Interrupt auf ne Flanke gehst, sollte das ganze einfacher sein.

Programmablauf wäre folgender:
- steigende Flanke -> Interrupt
- Interruptserviceroutine startet Timer
- nächste steigende Flanke -> Interrupt
- Timer anhalten und Wert auslesen.
- Wert mit Soll-Wert für 2ms (abzgl. vergleichen usw.)
- Wenn gleich, dann das eigentliche Programm starten
- sonst Timer rücksetzen und wieder starten
- und auf nächste steigende Flanke warten...