Save/restore PRIMASK register on Cortex-M3

This preserves the interrupt-enabled bit across context switches

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile
index b2413a2ce5801935e94ba825574134932439ef57..9e4f9e38acda8b53e78d2c25a74f1769f74517e1 100644 (file)
--- a/src/stm-demo/Makefile
+++ b/src/stm-demo/Makefile
@@ -3,8 +3,8 @@
 #
 #
 vpath % ..:../core:../product:../drivers:../stm
-vpath make-altitude ..
-vpath make-kalman ..
+vpath make-altitude ../util
+vpath make-kalman ../util
 vpath kalman.5c ../kalman
 vpath kalman_filter.5c ../kalman
 vpath load_csv.5c ../kalman
@@ -24,6 +24,7 @@ endif
 
 INC = \
        ao.h \
+       ao_arch.h \
        ao_pins.h \
        altitude.h \
        ao_kalman.h
@@ -48,7 +49,7 @@ PRODUCT_DEF=-DSTM_DEMO
 IDPRODUCT=0x000a
 CPU=cortex-m3
 CFLAGS = $(PRODUCT_DEF) -I. -I../stm -I../core -I..
-CFLAGS += -g -std=gnu99 -O0 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib -I../stm $(CINC)
+CFLAGS += -g -std=gnu99 -Os -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib -I../stm $(CINC)
 
 NICKLE=nickle
 
@@ -77,6 +78,8 @@ $(PROG): Makefile $(OBJ)
 ../altitude.h: make-altitude
        nickle $< > $@
 
+$(OBJ): $(INC)
+
 ao_product.h: ao-make-product.5c ../Version
        $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@
 

diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h
index 205f95d7eb4d7c71e6e73e43721b48ce4d6128ef..c4b98c7a4ef9ec0808d7b30d3c1d16b4c2f7bbd9 100644 (file)
--- a/src/stm/ao_arch.h
+++ b/src/stm/ao_arch.h
@@ -70,31 +70,44 @@ extern const uint16_t ao_serial_number;
                uint32_t        a = (uint32_t) start;                   \
                int             i;                                      \
                                                                        \
+               /* APSR */                                              \
+               ARM_PUSH32(sp, 0);                                      \
+                                                                       \
+               /* PRIMASK with interrupts enabled */                   \
+               ARM_PUSH32(sp, 0);                                      \
+                                                                       \
                /* Return address (goes into LR) */                     \
                ARM_PUSH32(sp, a);                                      \
                                                                        \
-               /* Clear register values  */                            \
+               /* Clear register values r0-r12 */                      \
                i = 13;                                                 \
                while (i--)                                             \
                        ARM_PUSH32(sp, 0);                              \
                                                                        \
-               /* APSR */                                              \
-               ARM_PUSH32(sp, 0);                                      \
                task->sp = sp;                                          \
 } while (0);
        
-#define ao_arch_save_regs() do {                                       \
+#define ao_arch_save_regs()    do {                                    \
+               uint32_t        apsr;                                   \
+               uint32_t        primask;                                \
+                                                                       \
+               /* Save APSR */                                         \
+               asm("mrs %0,apsr" : "=&r" (apsr));                      \
+               asm("push {%0}" : : "r" (apsr));                        \
+                                                                       \
+               /* Save PRIMASK */                                      \
+               asm("mrs %0,primask" : "=&r" (primask));                \
+               asm("push {%0}" : : "r" (primask));                     \
+                                                                       \
+               /* Save general registers */                            \
                asm("push {r0-r12,lr}\n");                              \
-               cli();                                                  \
-               asm("mrs r0,apsr" "\n\t" "push {r0}");                  \
-               sei();                                                  \
        } while (0)
 
 #define ao_arch_save_stack() do {                                      \
-               uint32_t        sp;                                     \
+               uint32_t        *sp;                                    \
                asm("mov %0,sp" : "=&r" (sp) );                         \
-               ao_cur_task->sp = (uint32_t *) (sp);                    \
-               if ((uint8_t *) ao_cur_task->sp < ao_cur_task->stack)   \
+               ao_cur_task->sp = (sp);                                 \
+               if ((uint8_t *) sp < ao_cur_task->stack)                \
                        ao_panic (AO_PANIC_STACK);                      \
        } while (0)
 
@@ -106,11 +119,25 @@ extern const uint16_t ao_serial_number;
 
 #define ao_arch_restore_stack() do { \
                uint32_t        sp;                                     \
+               uint32_t        primask;                                \
+               uint32_t        apsr;                                   \
                sp = (uint32_t) ao_cur_task->sp;                        \
-               cli();                                                  \
+                                                                       \
+               /* Switch stacks */                                     \
                asm("mov sp, %0" : : "r" (sp) );                        \
-               asm("pop {r0}" "\n\t" "msr apsr,r0");                   \
+                                                                       \
+               /* Restore general registers */                         \
                asm("pop {r0-r12,lr}\n");                               \
+                                                                       \
+               /* Restore PRIMASK */                                   \
+               asm("pop {%0}" : "=&r" (primask) );                     \
+               asm("msr primask,%0" : : "r" (primask) );               \
+                                                                       \
+               /* Restore APSR */                                      \
+               asm("pop {%0}" : "=&r" (apsr) );                        \
+               asm("msr apsr,%0" : : "r" (apsr) );                     \
+                                                                       \
+               /* Return to calling function */                        \
                asm("bx lr");                                           \
        } while(0)