X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Flpc%2Fao_arch_funcs.h;h=21a7a8e5702f779fd2ddce6e164e89a8bf77ab9e;hb=cb83b5432e1320726ad496ded78cced7274618b0;hp=9a3219a2e6275e55a6972dcdf5fff53a390b75c5;hpb=0dd148e388944d8d265da51d62806c4a00b2c13d;p=fw%2Faltos

diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h
index 9a3219a2..21a7a8e5 100644
--- a/src/lpc/ao_arch_funcs.h
+++ b/src/lpc/ao_arch_funcs.h
@@ -161,16 +161,17 @@ static inline void ao_arch_restore_stack(void) {
 
 #endif /* HAS_TASK */
 
-#define ao_arch_wait_interrupt() do {			\
-		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
-		ao_arch_release_interrupts();				\
-		ao_arch_block_interrupts();				\
+#define ao_arch_wait_interrupt() do {				\
+		asm("\twfi\n");					\
+		ao_arch_release_interrupts();			\
+		asm(".global ao_idle_loc\n\nao_idle_loc:");	\
+		ao_arch_block_interrupts();			\
 	} while (0)
 
-#define ao_arch_critical(b) do {				\
-		ao_arch_block_interrupts();			\
-		do { b } while (0);				\
-		ao_arch_release_interrupts();			\
+#define ao_arch_critical(b) do {			\
+		uint32_t __mask = ao_arch_irqsave();	\
+		do { b } while (0);			\
+		ao_arch_irqrestore(__mask);		\
 	} while (0)
 
 /*
@@ -235,6 +236,7 @@ static inline void ao_arch_start_scheduler(void) {
 	asm("mrs %0,control" : "=&r" (control));
 	control |= (1 << 1);
 	asm("msr control,%0" : : "r" (control));
+	asm("isb");
 }
 
 #endif /* _AO_ARCH_FUNCS_H_ */