2 files changed, 36 insertions, 19 deletions
diff --git a/src/app/transactiontest/main.cc b/src/app/transactiontest/main.cc
index cb2d86e..d0ef9bf 100644
--- a/src/app/transactiontest/main.cc
+++ b/src/app/transactiontest/main.cc
@@ -42,10 +42,12 @@ int main(void)
 	gpio.setup();
 	kout.setup();
 
-	//restore_state();
 
 	gpio.led_on(0);
 	gpio.input(GPIO::p4_5, 1);
+
+	asm_load_all();
+
 	kout << "Hello, World!" << endl;
 
 	arch.idle_loop();
diff --git a/src/app/transactiontest/util.S b/src/app/transactiontest/util.S
index c0b1ebc..32744f8 100644
--- a/src/app/transactiontest/util.S
+++ b/src/app/transactiontest/util.S
@@ -8,12 +8,18 @@ stack_backup:
 sp_backup:
     .space 2
 
+backup_cookie:
+    .space 2
+
 asm_save_all:
     dint
     .irp reg,4,5,6,7,8,9,10,11
     push r\reg
     .endr
 
+    mov #1234h, r10
+    mov r10, &backup_cookie
+
     mov r1, &sp_backup
 
     mov #1c00h, r10
@@ -34,6 +40,15 @@ save_sram_word:
 asm_load_all:
     dint
 
+    push r11
+    mov &backup_cookie, r11
+    cmp #1234h, r11
+    jeq do_load_all
+    pop r11
+    eint
+    ret
+
+do_load_all:
     ; restore SRAM from backup
     mov #stack_backup, r10
     mov #1c00h, r11
@@ -59,32 +74,32 @@ load_sram_word:
 
 asm_load_mem:
     dint
-    ; save return address in r4
-    mov @r1, r4
 
-    ; restore SRAM from backup
+    push r11
+    mov &backup_cookie, r11
+    cmp #1234h, r11
+    jeq do_load_mem
+    pop r11
+    eint
+    ret
+
+do_load_mem:
+    push r10
+    push r9
+    ; restore SRAM from backup, excluding current stack frame
+    mov r1, r9
+    sub 6, r9
     mov #stack_backup, r10
     mov #1c00h, r11
 load_sram_word2:
     mov @r10+, 0(r11)
     add #2, r11
-    cmp #1c00h+2048, r11
+    cmp r9, r11
     jlo load_sram_word2
 
-    ; restore stack pointer
-    mov &sp_backup, r1
-
-    ; restore old register contents
-    .irp reg,11,10,9,8,7,6,5
-    pop r\reg
-    .endr
-
-    ; load return address, that is, the address this function was called from.
-    ; This is not the address which called asm_save_all -- we only want to restore memory contents, not re-execute everything
-    mov r4, 2(r1)
-
-    ; load remaining register content
-    pop r4
+    pop r9
+    pop r10
+    pop r11
 
     eint
     ret