Ok, now before I start to mess around again in the code, 
I suggest to move the `restoreIRQ(...)` from line 145 to the else branch below line 141?