Adding retore function, confirmed works
modifications to test program, confirmed works
Changes made, need to check CPSR on entry into SWI
Fixing all merge issues, TODO: CPSR during SWI