Fixed swi_handler bug not restoring user stack