used add.w #2,SP instead of pop r3; verified code is bug free