From 2df02ed0ad6b9a6002cc114e97d18844ed8e4c20 Mon Sep 17 00:00:00 2001
From: Anup Patel <anup@brainfault.org>
Date: Tue, 5 Jul 2011 14:27:30 +0530
Subject: [PATCH] Fixed CRITICAL section macros.

Signed-off-by: Anup Patel <anup@brainfault.org>
---
 ports/arm7a/arm_irq.c   | 55 +++++++++++++++++++++++++++++++++++++++++
 ports/arm7a/arm_irq.h   |  2 ++
 ports/arm7a/arm_main.c  |  2 --
 ports/arm7a/arm_timer.c |  6 -----
 ports/arm7a/atomport.h  | 34 +++----------------------
 5 files changed, 61 insertions(+), 38 deletions(-)

diff --git a/ports/arm7a/arm_irq.c b/ports/arm7a/arm_irq.c
index 153bad2..1aec38c 100644
--- a/ports/arm7a/arm_irq.c
+++ b/ports/arm7a/arm_irq.c
@@ -36,22 +36,47 @@ arm_irq_handler_t irq_hndls[NR_IRQS_PBA8];
 
 void do_undefined_instruction(pt_regs_t *regs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_software_interrupt(pt_regs_t *regs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_prefetch_abort(pt_regs_t *regs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_data_abort(pt_regs_t *regs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_not_used(pt_regs_t *regs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_irq(pt_regs_t *uregs)
@@ -59,6 +84,9 @@ void do_irq(pt_regs_t *uregs)
 	int rc = 0;
 	int irq = arm_gic_active_irq(0);
 
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
 	if (-1 < irq) {
 		if (irq_hndls[irq]) {
 			rc = irq_hndls[irq](irq, uregs);
@@ -71,10 +99,18 @@ void do_irq(pt_regs_t *uregs)
 			while (1);
 		}
 	}
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void do_fiq(pt_regs_t *uregs)
 {
+	/* Call the interrupt entry routine */
+	atomIntEnter();
+
+	/* Call the interrupt exit routine */
+	atomIntExit(TRUE);
 }
 
 void arm_irq_setup(void)
@@ -149,3 +185,22 @@ void arm_irq_disable(void)
 	__asm( "cpsid if" );
 }
 
+irq_flags_t arm_irq_save(void)
+{
+	unsigned long retval;
+
+	asm volatile (" mrs     %0, cpsr\n\t" " cpsid   i"	/* Syntax CPSID <iflags> {, #<p_mode>}
+								 * Note: This instruction is supported 
+								 * from ARM6 and above
+								 */
+		      :"=r" (retval)::"memory", "cc");
+
+	return retval;
+}
+
+void arm_irq_restore(irq_flags_t flags)
+{
+	asm volatile (" msr     cpsr_c, %0"::"r" (flags)
+		      :"memory", "cc");
+}
+
diff --git a/ports/arm7a/arm_irq.h b/ports/arm7a/arm_irq.h
index bad336d..1f0431e 100644
--- a/ports/arm7a/arm_irq.h
+++ b/ports/arm7a/arm_irq.h
@@ -51,5 +51,7 @@ void arm_irq_setup(void);
 void arm_irq_register(uint32_t irq_no, arm_irq_handler_t hndl);
 void arm_irq_enable(void);
 void arm_irq_disable(void);
+irq_flags_t arm_irq_save(void);
+void arm_irq_restore(irq_flags_t flags);
 
 #endif /* __ARM_IRQ_H */
diff --git a/ports/arm7a/arm_main.c b/ports/arm7a/arm_main.c
index 7cfd526..cb324f8 100644
--- a/ports/arm7a/arm_main.c
+++ b/ports/arm7a/arm_main.c
@@ -177,8 +177,6 @@ int main ( void )
     {
 	arm_irq_setup();
 
-	arm_irq_enable();
-
 	arm_timer_init(1000, 1);
 
 	arm_uart_init();
diff --git a/ports/arm7a/arm_timer.c b/ports/arm7a/arm_timer.c
index 8a64676..ed5b359 100644
--- a/ports/arm7a/arm_timer.c
+++ b/ports/arm7a/arm_timer.c
@@ -62,15 +62,9 @@ void arm_timer_clearirq(void)
 
 int arm_timer_irqhndl(uint32_t irq_no, pt_regs_t * regs)
 {
-	/* Call the interrupt entry routine */
-	atomIntEnter();
-
 	/* Call the OS system tick handler */
 	atomTimerTick();
 
-	/* Call the interrupt exit routine */
-	atomIntExit(TRUE);
-
 	arm_timer_clearirq();
 
 	return 0;
diff --git a/ports/arm7a/atomport.h b/ports/arm7a/atomport.h
index 73b19e3..debf68a 100644
--- a/ports/arm7a/atomport.h
+++ b/ports/arm7a/atomport.h
@@ -71,38 +71,12 @@ struct pt_regs {
 typedef struct pt_regs pt_regs_t;
 
 #include <printk.h>
+#include <arm_irq.h>
 
-extern uint32_t at_preempt_count;
-
-#if 0
 /* Critical region protection */
-#define CRITICAL_STORE	    uint32_t status_reg
-#define CRITICAL_START()					\
-	do {							\
-		extern uint32_t at_preempt_count;		\
-		at_preempt_count++;				\
-	}while(0);
-
-#define CRITICAL_END()							\
-	do {								\
-		extern uint32_t at_preempt_count;			\
-		if (at_preempt_count == 0) {				\
-			printk("BUG: Preempt count is zero!\n");	\
-			for(;;);					\
-		}							\
-		at_preempt_count--;					\
-									\
-		if (at_preempt_count == 0) {				\
-			if (atomCurrentContext()) {			\
-			}						\
-		}							\
-									\
-	}while(0);
-#else
-#define CRITICAL_STORE
-#define CRITICAL_START()
-#define CRITICAL_END()
-#endif
+#define CRITICAL_STORE		irq_flags_t status_flags
+#define CRITICAL_START()	status_flags = arm_irq_save();
+#define CRITICAL_END()		arm_irq_restore(status_flags);
 
 /* Uncomment to enable stack-checking */
 /* #define ATOM_STACK_CHECKING */