Context switch correction

Added a intermediate step to save the current stkpointer.