diff --git a/libcortex_a9/src/lib.rs b/libcortex_a9/src/lib.rs
index aa847a3..e443bad 100644
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@@ -36,7 +36,9 @@ pub fn notify_spin_lock() {
 }
 
 #[macro_export]
-/// Interrupt handler, which setup the stack and jump to actual interrupt handler.
+/// Interrupt handler, which setup the stack and preserve registers before jumping to actual interrupt handler.
+/// Registers r0-r12, PC, SP and CPSR are restored after the actual handler.
+/// 
 /// - `name` is the name of the interrupt, should be the same as the one defined in vector table.
 /// - `name2` is the name for the actual handler, should be different from name.
 /// - `stack0` is the stack for the interrupt handler when called from core0.
@@ -44,8 +46,7 @@ pub fn notify_spin_lock() {
 /// - `body` is the body of the actual interrupt handler, should be a normal unsafe rust function
 /// body.
 ///
-/// Note that the interrupt handler would use the same stack as normal programs by default, so
-/// interrupt handlers should not return to normal program or it may corrupt the stack.
+/// Note that the interrupt handler would use the same stack as normal programs by default.
 macro_rules! interrupt_handler {
     ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
         #[link_section = ".text.boot"]
@@ -54,19 +55,27 @@ macro_rules! interrupt_handler {
         pub unsafe extern "C" fn $name() -> ! {
             asm!(
                 // setup SP, depending on CPU 0 or 1
+                // and preserve registers
+                "SUB lr, lr, #4",
+                "STMFD sp!, {{r0-r12, lr}}",
                 "mrc p15, #0, r0, c0, c0, #5",
                 concat!("movw r1, :lower16:", stringify!($stack0)),
                 concat!("movt r1, :upper16:", stringify!($stack0)),
                 "tst r0, #3",
                 concat!("movwne r1, :lower16:", stringify!($stack1)),
                 concat!("movtne r1, :upper16:", stringify!($stack1)),
+                "mov r0, sp",
                 "mov sp, r1",
+                "push {{r0, r1}}",                                      // for stack alignment 
                 concat!("bl ", stringify!($name2)),
+                "pop {{r0, r1}}",
+                "mov sp, r0",
+                "LDMFD sp!, {{r0-r12, pc}}^",                           // caret ^ : copy SPSR to the CPSR  
                 options(noreturn)
             );
         }
 
         #[no_mangle]
-        pub unsafe extern "C" fn $name2() -> ! $body
+        pub unsafe extern "C" fn $name2() $body
     };
 }