altos: Clean up stm arch macros a bit.

Turn a bunch of the macros into inline functions.
Clean up the reboot method to use the stm_scb structure.

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

4 files changed, 134 insertions, 109 deletions

diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h
index 0c3cfc91..e270199e 100644
--- a/src/stm/ao_arch.h
+++ b/src/stm/ao_arch.h
@@ -46,9 +46,9 @@
 #define __interrupt(n)
 #define __at(n)
 
-#define CORTEX_M3_AIRCR		((uint32_t *) 0xe000ed0c)
-
-#define ao_arch_reboot()	(*((uint32_t *) 0xe000ed0c) = 0x05fa0004)
+#define ao_arch_reboot() \
+	(stm_scb.aircr = ((STM_SCB_AIRCR_VECTKEY_KEY << STM_SCB_AIRCR_VECTKEY) | \
+			  (1 << STM_SCB_AIRCR_SYSRESETREQ)))
 
 #define ao_arch_nop()		asm("nop")
 
@@ -77,118 +77,12 @@ extern const uint16_t ao_romconfig_check;
 extern const uint16_t ao_serial_number;
 extern const uint32_t ao_radio_cal;
 
-#define ARM_PUSH32(stack, val)	(*(--(stack)) = (val))
-
 #define ao_arch_task_members\
 	uint32_t *sp;			/* saved stack pointer */
 
 #define ao_arch_block_interrupts()	asm("cpsid i")
 #define ao_arch_release_interrupts()	asm("cpsie i")
 
-#define cli()	ao_arch_block_interrupts()
-#define sei()	ao_arch_release_interrupts()
-
-static uint32_t
-ao_arch_irqsave(void) {
-	uint32_t	primask;
-	asm("mrs %0,primask" : "=&r" (primask));
-	ao_arch_block_interrupts();
-	return primask;
-}
-
-static void
-ao_arch_irqrestore(uint32_t primask) {
-	asm("msr primask,%0" : : "r" (primask));
-}
-
-static void
-ao_arch_memory_barrier() {
-	asm volatile("" ::: "memory");
-}
-
-#define ao_arch_init_stack(task, start) do {				\
-		uint32_t	*sp = (uint32_t *) (task->stack + AO_STACK_SIZE); \
-		uint32_t	a = (uint32_t) start; 			\
-		int		i;					\
-									\
-		/* Return address (goes into LR) */			\
-		ARM_PUSH32(sp, a);					\
-									\
-		/* Clear register values r0-r12 */			\
-		i = 13;							\
-		while (i--)						\
-			ARM_PUSH32(sp, 0);				\
-									\
-		/* APSR */						\
-		ARM_PUSH32(sp, 0);					\
-									\
-		/* PRIMASK with interrupts enabled */			\
-		ARM_PUSH32(sp, 0);					\
-									\
-		task->sp = sp;						\
-} while (0);
-	
-#define ao_arch_save_regs() 	do {					\
-		/* Save general registers */				\
-		asm("push {r0-r12,lr}\n");				\
-									\
-		/* Save APSR */						\
-		asm("mrs r0,apsr");					\
-		asm("push {r0}");					\
-									\
-		/* Save PRIMASK */ 					\
-		asm("mrs r0,primask");					\
-		asm("push {r0}");					\
-									\
-		/* Enable interrupts */					\
-		sei();							\
-	} while (0)
-
-#define ao_arch_save_stack() do {					\
-		uint32_t	*sp;					\
-		asm("mov %0,sp" : "=&r" (sp) );				\
-		ao_cur_task->sp = (sp);					\
-		if ((uint8_t *) sp < &ao_cur_task->stack[0])		\
-			ao_panic (AO_PANIC_STACK);			\
-	} while (0)
-
-#if 0
-#define ao_arch_isr_stack() do {				\
-		uint32_t	*sp = (uint32_t *) 0x20004000;	\
-		asm("mov %0,sp" : "=&r" (sp) );			\
-	} while (0)
-#else
-#define ao_arch_isr_stack()
-#endif
-
-
-#define ao_arch_cpu_idle() do {			\
-		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
-	} while (0)
-
-#define ao_arch_restore_stack() do { \
-		uint32_t	sp;					\
-		sp = (uint32_t) ao_cur_task->sp;			\
-									\
-		/* Switch stacks */					\
-		asm("mov sp, %0" : : "r" (sp) );			\
-									\
-		/* Restore PRIMASK */					\
-		asm("pop {r0}");					\
-		asm("msr primask,r0");					\
-									\
-		/* Restore APSR */					\
-		asm("pop {r0}");					\
-		asm("msr apsr,r0");					\
-									\
-		/* Restore general registers */				\
-		asm("pop {r0-r12,lr}\n");				\
-									\
-		/* Return to calling function */			\
-		asm("bx lr");						\
-	} while(0)
-
-#define ao_arch_critical(b) do { cli(); do { b } while (0); sei(); } while (0)
 
 /*
  * For now, we're running at a weird frequency
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index e66d20d7..ca451a53 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -210,4 +210,103 @@ ao_i2c_recv(void *block, uint16_t len, uint8_t i2c_index, uint8_t stop);
 void
 ao_i2c_init(void);
 
+#define ARM_PUSH32(stack, val)	(*(--(stack)) = (val))
+
+static inline uint32_t
+ao_arch_irqsave(void) {
+	uint32_t	primask;
+	asm("mrs %0,primask" : "=&r" (primask));
+	ao_arch_block_interrupts();
+	return primask;
+}
+
+static inline void
+ao_arch_irqrestore(uint32_t primask) {
+	asm("msr primask,%0" : : "r" (primask));
+}
+
+static inline void
+ao_arch_memory_barrier() {
+	asm volatile("" ::: "memory");
+}
+
+static inline void
+ao_arch_init_stack(struct ao_task *task, void *start)
+{
+	uint32_t	*sp = (uint32_t *) (task->stack + AO_STACK_SIZE);
+	uint32_t	a = (uint32_t) start;
+	int		i;
+
+	/* Return address (goes into LR) */
+	ARM_PUSH32(sp, a);
+
+	/* Clear register values r0-r12 */
+	i = 13;
+	while (i--)
+		ARM_PUSH32(sp, 0);
+
+	/* APSR */
+	ARM_PUSH32(sp, 0);
+
+	/* PRIMASK with interrupts enabled */
+	ARM_PUSH32(sp, 0);
+
+	task->sp = sp;
+}
+
+static inline void ao_arch_save_regs(void) {
+	/* Save general registers */
+	asm("push {r0-r12,lr}\n");
+
+	/* Save APSR */
+	asm("mrs r0,apsr");
+	asm("push {r0}");
+
+	/* Save PRIMASK */
+	asm("mrs r0,primask");
+	asm("push {r0}");
+}
+
+static inline void ao_arch_save_stack(void) {
+	uint32_t	*sp;
+	asm("mov %0,sp" : "=&r" (sp) );
+	ao_cur_task->sp = (sp);
+	if ((uint8_t *) sp < &ao_cur_task->stack[0])
+		ao_panic (AO_PANIC_STACK);
+}
+
+static inline void ao_arch_restore_stack(void) {
+	uint32_t	sp;
+	sp = (uint32_t) ao_cur_task->sp;
+
+	/* Switch stacks */
+	asm("mov sp, %0" : : "r" (sp) );
+
+	/* Restore PRIMASK */
+	asm("pop {r0}");
+	asm("msr primask,r0");
+
+	/* Restore APSR */
+	asm("pop {r0}");
+	asm("msr apsr,r0");
+
+	/* Restore general registers */
+	asm("pop {r0-r12,lr}\n");
+
+	/* Return to calling function */
+	asm("bx lr");
+}
+
+#define ao_arch_isr_stack()
+
+#define ao_arch_cpu_idle() do {			\
+		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
+	} while (0)
+
+#define ao_arch_critical(b) do {				\
+		ao_arch_block_interrupts();			\
+		do { b } while (0);				\
+		ao_arch_release_interrupts();			\
+	} while (0)
+
 #endif /* _AO_ARCH_FUNCS_H_ */
diff --git a/src/stm/registers.ld b/src/stm/registers.ld
index f8b224a2..95a86e35 100644
--- a/src/stm/registers.ld
+++ b/src/stm/registers.ld
@@ -46,6 +46,8 @@ stm_tim2   = 0x40000000;
 
 stm_nvic   = 0xe000e100;
 
+stm_scb    = 0xe000ed00;
+
 stm_mpu    = 0xe000ed90;
 
 /* calibration data in system memory */
diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h
index d953aee4..0dbfae39 100644
--- a/src/stm/stm32l.h
+++ b/src/stm/stm32l.h
@@ -901,6 +901,36 @@ stm_nvic_get_priority(int irq) {
 	return (stm_nvic.ipr[IRQ_PRIO_REG(irq)] >> IRQ_PRIO_BIT(irq)) & IRQ_PRIO_MASK(0);
 }
 
+struct stm_scb {
+	vuint32_t	cpuid;
+	vuint32_t	icsr;
+	vuint32_t	vtor;
+	vuint32_t	aircr;
+
+	vuint32_t	scr;
+	vuint32_t	ccr;
+	vuint32_t	shpr1;
+	vuint32_t	shpr2;
+
+	vuint32_t	shpr3;
+	vuint32_t	shcrs;
+	vuint32_t	cfsr;
+	vuint32_t	hfsr;
+
+	uint32_t	unused_30;
+	vuint32_t	mmfar;
+	vuint32_t	bfar;
+};
+
+extern struct stm_scb stm_scb;
+
+#define STM_SCB_AIRCR_VECTKEY		16
+#define  STM_SCB_AIRCR_VECTKEY_KEY		0x05fa
+#define STM_SCB_AIRCR_PRIGROUP		8
+#define STM_SCB_AIRCR_SYSRESETREQ	2
+#define STM_SCB_AIRCR_VECTCLRACTIVE	1
+#define STM_SCB_AIRCR_VECTRESET		0
+
 struct stm_mpu {
 	vuint32_t	typer;
 	vuint32_t	cr;