Und wo liegt jetzt Dein Problem ?
Wenn Du denkst, daß Deine wait() Funktion auch noch was tut dann irrst Du gewaltig...
füge in der Funktion __asm volatile ("nop); ein, dann soltte es besser klappen.

Warum das so ist, wirst Du in der Wiki rausfinden.

Gruß Sebastian

EDIT:
@Hanni, ups, hehe ja genau, den muß er erst als Ausgang deklarieren