restore SP when exit

bsp/init0.c

@@ -1,15 +1,17 @@
-/* Exec after reset  */
+#include <bootloader.h>
+
+#ifdef ENABLE_OPTION_BOOTLOADER
 void bootloader_enter(void) __trap __naked {
+    /* Exec after reset  */
 __asm
-    // ldw X, #0x03FF
-    // ldw SP, X
+    ldw Y, SP           ; [90 96] 
     ldw X, #0x483F      ; [AE 48 3F]
     _cycle:
         decw X          ; [5A]
         push A          ; [88]
         ld A, (X)       ; [F6]
         jrne _cycle     ; [26 FB]
-        // check 
+        // check address
         ldw X, SP       ; [96]
         addw X, #3      ; [1C 00 03]
         cpw X, (1,SP)   ; [13 01]
@@ -17,6 +19,8 @@ __asm
         // jump to ram
         ret             ; [81]
     _exit:
+        ldw SP,Y        ; [90 94]
         jp 0x8004       ; [cc 80 04]
 __endasm;
 }
+#endif /* ENABLE_OPTION_BOOTLOADER */

src/main.c

@@ -1,6 +1,7 @@
 #include <stdint.h>
 #include <stm8s.h>
 #include <delay.h>
+#include <bootloader.h>
 
 #define LED_PIN     5