altos: Make stm-flash capable of switching to application

[fw/altos] / src / stm / ao_interrupt.c

diff --git a/src/stm/ao_interrupt.c b/src/stm/ao_interrupt.c
index 12763a305737623de75ad2c61757cbb9f445567e..9e7562193c37d1567b11ef5fd1d8ed56ad5671cf 100644 (file)
--- a/src/stm/ao_interrupt.c
+++ b/src/stm/ao_interrupt.c
@@ -42,12 +42,45 @@ const void *stm_interrupt_vector[];
 
 #ifdef AO_BOOT_APPLICATION_PIN
 #include <ao_exti.h>
+
+#define AO_BOOT_APPLICATION            0x5a5aa5a5
+#define AO_BOOT_APPLICATION_CHECK      0xc3c33c3c
+
+static uint32_t        ao_boot_application;
+static uint32_t        ao_boot_application_check;
+
+static void
+ao_boot_chain(void) {
+       uint32_t        sp;
+       uint32_t        pc;
+
+       sp = BOOT_FETCH(0);
+       pc = BOOT_FETCH(4);
+       asm ("mov sp, %0" : : "r" (sp));
+       asm ("mov lr, %0" : : "r" (pc));
+       asm ("bx lr");
+}
+
+void
+ao_reboot_application(void)
+{
+       ao_boot_application = AO_BOOT_APPLICATION;
+       ao_boot_application_check = AO_BOOT_APPLICATION_CHECK;
+       ao_arch_reboot();
+}
+
 #endif
 
 void start(void) {
 #ifdef AO_BOOT_APPLICATION_PIN
        uint16_t v;
 
+       if (ao_boot_application == AO_BOOT_APPLICATION &&
+           ao_boot_application_check == AO_BOOT_APPLICATION_CHECK) {
+               ao_boot_application = 0;
+               ao_boot_application_check = 0;
+               ao_boot_chain();
+       }
        /* Enable power interface clock */
        stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_PWREN);
        
@@ -63,16 +96,7 @@ void start(void) {
        ao_disable_port(&AO_BOOT_APPLICATION_GPIO);
        stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_PWREN);
        if (v == AO_BOOT_APPLICATION_VALUE)
-       {
-               uint32_t        sp;
-               uint32_t        pc;
-
-               sp = BOOT_FETCH(0);
-               pc = BOOT_FETCH(4);
-               asm ("mov sp, %0" : : "r" (sp));
-               asm ("mov lr, %0" : : "r" (pc));
-               asm ("bx lr");
-       }
+               ao_boot_chain();
 #endif
 
        /* Set interrupt vector table offset */