From 43293c693791467201719806ea3c19838eaccebe Mon Sep 17 00:00:00 2001
From: Olivier Houchard <cognet@FreeBSD.org>
Date: Sun, 9 Apr 2006 20:16:47 +0000
Subject: [PATCH] Not only disable/enable interrupts, do it for FIQs as well,
 when needed.

---
 sys/arm/arm/swtch.S     |  2 +-
 sys/arm/arm/trap.c      | 31 ++++++++++++++++++++++---------
 sys/arm/arm/undefined.c |  2 +-
 3 files changed, 24 insertions(+), 11 deletions(-)

diff --git a/sys/arm/arm/swtch.S b/sys/arm/arm/swtch.S
index 2b54874c3f5c..111f45955558 100644
--- a/sys/arm/arm/swtch.S
+++ b/sys/arm/arm/swtch.S
@@ -467,7 +467,7 @@ ENTRY(fork_trampoline)
 	bl	_C_LABEL(fork_exit)
 	/* Kill irq"s */
 	mrs	r0, cpsr
-	orr	r0, r0, #(I32_bit)
+	orr	r0, r0, #(I32_bit|F32_bit)
 	msr	cpsr_c, r0
 	DO_AST
 	PULLFRAME
diff --git a/sys/arm/arm/trap.c b/sys/arm/arm/trap.c
index 0c439d2a96b2..b1534fe5d092 100644
--- a/sys/arm/arm/trap.c
+++ b/sys/arm/arm/trap.c
@@ -271,9 +271,13 @@ data_abort_handler(trapframe_t *tf)
 	/* Grab the current pcb */
 	pcb = td->td_pcb;
 	/* Re-enable interrupts if they were enabled previously */
-	if (td->td_md.md_spinlock_count == 0 &&
-	    __predict_true(tf->tf_spsr & I32_bit) == 0)
-		enable_interrupts(I32_bit);
+	if (td->td_md.md_spinlock_count == 0) {
+		if (__predict_true(tf->tf_spsr & I32_bit) == 0)
+			enable_interrupts(I32_bit);
+		if (__predict_true(tf->tf_spsr & F32_bit) == 0)
+			enable_interrupts(F32_bit);
+	}
+		
 
 	/* Invoke the appropriate handler, if necessary */
 	if (__predict_false(data_aborts[fsr & FAULT_TYPE_MASK].func != NULL)) {
@@ -487,6 +491,7 @@ dab_fatal(trapframe_t *tf, u_int fsr, u_int far, struct thread *td, struct ksig
 
 	mode = TRAP_USERMODE(tf) ? "user" : "kernel";
 
+	disable_interrupts(I32_bit|F32_bit);
 	if (td != NULL) {
 		printf("Fatal %s mode data abort: '%s'\n", mode,
 		    data_aborts[fsr & FAULT_TYPE_MASK].desc);
@@ -730,9 +735,13 @@ prefetch_abort_handler(trapframe_t *tf)
 			thread_user_enter(td);
 	}
 	fault_pc = tf->tf_pc;
-	if (td->td_md.md_spinlock_count == 0 &&
-	    __predict_true((tf->tf_spsr & I32_bit) == 0))
-		enable_interrupts(I32_bit);
+	if (td->td_md.md_spinlock_count == 0) {
+		if (__predict_true(tf->tf_spsr & I32_bit) == 0)
+			enable_interrupts(I32_bit);
+		if (__predict_true(tf->tf_spsr & F32_bit) == 0)
+			enable_interrupts(F32_bit);
+	}
+	 
 
 		       
 	/* See if the cpu state needs to be fixed up */
@@ -1012,9 +1021,13 @@ swi_handler(trapframe_t *frame)
 	 * Since all syscalls *should* come from user mode it will always
 	 * be safe to enable them, but check anyway. 
 	 */       
-	if (td->td_md.md_spinlock_count == 0 && !(frame->tf_spsr & I32_bit))
-		enable_interrupts(I32_bit);
-
+	if (td->td_md.md_spinlock_count == 0) {
+		if (__predict_true(frame->tf_spsr & I32_bit) == 0)
+			enable_interrupts(I32_bit);
+		if (__predict_true(frame->tf_spsr & F32_bit) == 0)
+			enable_interrupts(F32_bit);
+	}
+	 
 	syscall(td, frame, insn);
 }
 
diff --git a/sys/arm/arm/undefined.c b/sys/arm/arm/undefined.c
index 21973ce8a317..25246a05e023 100644
--- a/sys/arm/arm/undefined.c
+++ b/sys/arm/arm/undefined.c
@@ -189,7 +189,7 @@ undefinedinstruction(trapframe_t *frame)
 
 	/* Enable interrupts if they were enabled before the exception. */
 	if (!(frame->tf_spsr & I32_bit))
-		enable_interrupts(I32_bit);
+		enable_interrupts(I32_bit|F32_bit);
 
 	frame->tf_pc -= INSN_SIZE;
 	PCPU_LAZY_INC(cnt.v_trap);