From faf2bea4fdb3dca7fbed35423a4fe4459ee92ceb Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Mon, 19 Mar 2012 11:25:40 -0700
Subject: [PATCH] Save/restore PRIMASK register on Cortex-M3

This preserves the interrupt-enabled bit across context switches

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/stm-demo/Makefile |  9 +++++---
 src/stm/ao_arch.h     | 51 +++++++++++++++++++++++++++++++++----------
 2 files changed, 45 insertions(+), 15 deletions(-)

diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile
index b2413a2c..9e4f9e38 100644
--- 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 205f95d7..c4b98c7a 100644
--- 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)
 
-- 
2.30.2