Und Du bist Dir sicher, daß da 40 kHz rauskommt ?
Ich würde doch reine Hardware Lösung vorziehen, oder zumindest ohne Interrupt auszukommen!
Wenn es wirklich so ist, daß Basic alle Register + SREG bei ISR auf den Stack legt fehlen Dir ein paar Takte Bild  

Gruß Sebastian