Bugfixes

1. ARM/Thumb switching fix
2. Fix for loosing step intos