Fix in context switch from Yield

Letting PendSV to do the actual switch, and keeping the rest to all but.