3 files changed, 28 insertions, 55 deletions

diff --git a/arch/xtensa/kernel/entry.S b/arch/xtensa/kernel/entry.S
index 3729b48d798d..5082507d5631 100644
--- a/arch/xtensa/kernel/entry.S
+++ b/arch/xtensa/kernel/entry.S
@@ -354,16 +354,16 @@ common_exception:
 	 * so we can allow exceptions and interrupts (*) again.
 	 * Set PS(EXCM = 0, UM = 0, RING = 0, OWB = 0, WOE = 1, INTLEVEL = X)
 	 *
-	 * (*) We only allow interrupts of higher priority than current IRQ
+	 * (*) We only allow interrupts if they were previously enabled and
+	 *     we're not handling an IRQ
 	 */
 
 	rsr	a3, ps
-	addi	a0, a0, -4
-	movi	a2, 1
+	addi	a0, a0, -EXCCAUSE_LEVEL1_INTERRUPT
+	movi	a2, LOCKLEVEL
 	extui	a3, a3, PS_INTLEVEL_SHIFT, PS_INTLEVEL_WIDTH
 					# a3 = PS.INTLEVEL
-	movnez	a2, a3, a3		# a2 = 1: level-1, > 1: high priority
-	moveqz	a3, a2, a0		# a3 = IRQ level iff interrupt
+	moveqz	a3, a2, a0		# a3 = LOCKLEVEL iff interrupt
 	movi	a2, 1 << PS_WOE_BIT
 	or	a3, a3, a2
 	rsr	a0, exccause
@@ -444,6 +444,8 @@ common_exception_return:
 1:	l32i	a3, a1, PT_PS
 	_bbci.l	a3, PS_UM_BIT, 4f
 
+	rsil	a2, 0
+
 	/* Specific to a user exception exit:
 	 * We need to check some flags for signal handling and rescheduling,
 	 * and have to restore WB and WS, extra states, and all registers
@@ -684,51 +686,19 @@ common_exception_exit:
 
 	l32i	a0, a1, PT_DEPC
 	l32i	a3, a1, PT_AREG3
-	_bltui	a0, VALID_DOUBLE_EXCEPTION_ADDRESS, 1f
-
-	wsr	a0, depc
 	l32i	a2, a1, PT_AREG2
-	l32i	a0, a1, PT_AREG0
-	l32i	a1, a1, PT_AREG1
-	rfde
+	_bgeui	a0, VALID_DOUBLE_EXCEPTION_ADDRESS, 1f
 
-1:
 	/* Restore a0...a3 and return */
 
-	rsr	a0, ps
-	extui	a2, a0, PS_INTLEVEL_SHIFT, PS_INTLEVEL_WIDTH
-	movi	a0, 2f
-	slli	a2, a2, 4
-	add	a0, a2, a0
-	l32i	a2, a1, PT_AREG2
-	jx	a0
-
-	.macro	irq_exit_level level
-	.align	16
-	.if	XCHAL_EXCM_LEVEL >= \level
-	l32i	a0, a1, PT_PC
-	wsr	a0, epc\level
 	l32i	a0, a1, PT_AREG0
 	l32i	a1, a1, PT_AREG1
-	rfi	\level
-	.endif
-	.endm
+	rfe
 
-	.align	16
-2:
+1: 	wsr	a0, depc
 	l32i	a0, a1, PT_AREG0
 	l32i	a1, a1, PT_AREG1
-	rfe
-
-	.align	16
-	/* no rfi for level-1 irq, handled by rfe above*/
-	nop
-
-	irq_exit_level 2
-	irq_exit_level 3
-	irq_exit_level 4
-	irq_exit_level 5
-	irq_exit_level 6
+	rfde
 
 ENDPROC(kernel_exception)
 
diff --git a/arch/xtensa/kernel/traps.c b/arch/xtensa/kernel/traps.c
index cf065e165ceb..30e53e609104 100644
--- a/arch/xtensa/kernel/traps.c
+++ b/arch/xtensa/kernel/traps.c
@@ -196,7 +196,6 @@ void do_multihit(struct pt_regs *regs, unsigned long exccause)
 
 /*
  * IRQ handler.
- * PS.INTLEVEL is the current IRQ priority level.
  */
 
 extern void do_IRQ(int, struct pt_regs *);
@@ -213,18 +212,21 @@ void do_interrupt(struct pt_regs *regs)
 		XCHAL_INTLEVEL6_MASK,
 		XCHAL_INTLEVEL7_MASK,
 	};
-	unsigned level = get_sr(ps) & PS_INTLEVEL_MASK;
-
-	if (WARN_ON_ONCE(level >= ARRAY_SIZE(int_level_mask)))
-		return;
 
 	for (;;) {
 		unsigned intread = get_sr(interrupt);
 		unsigned intenable = get_sr(intenable);
-		unsigned int_at_level = intread & intenable &
-			int_level_mask[level];
+		unsigned int_at_level = intread & intenable;
+		unsigned level;
+
+		for (level = LOCKLEVEL; level > 0; --level) {
+			if (int_at_level & int_level_mask[level]) {
+				int_at_level &= int_level_mask[level];
+				break;
+			}
+		}
 
-		if (!int_at_level)
+		if (level == 0)
 			return;
 
 		/*
diff --git a/arch/xtensa/kernel/vectors.S b/arch/xtensa/kernel/vectors.S
index a7e1d0834c68..f9e175382aa9 100644
--- a/arch/xtensa/kernel/vectors.S
+++ b/arch/xtensa/kernel/vectors.S
@@ -386,9 +386,12 @@ ENDPROC(_DebugInterruptVector)
 	.if	XCHAL_EXCM_LEVEL >= \level
 	.section .Level\level\()InterruptVector.text, "ax"
 ENTRY(_Level\level\()InterruptVector)
-	wsr	a0, epc1
+	wsr	a0, excsave2
 	rsr	a0, epc\level
-	xsr	a0, epc1
+	wsr	a0, epc1
+	movi	a0, EXCCAUSE_LEVEL1_INTERRUPT
+	wsr	a0, exccause
+	rsr	a0, eps\level
 					# branch to user or kernel vector
 	j	_SimulateUserKernelVectorException
 	.endif
@@ -440,10 +443,8 @@ ENDPROC(_WindowOverflow4)
 	 */
 	.align 4
 _SimulateUserKernelVectorException:
-	wsr	a0, excsave2
-	movi	a0, 4			# LEVEL1_INTERRUPT cause
-	wsr	a0, exccause
-	rsr	a0, ps
+	addi	a0, a0, (1 << PS_EXCM_BIT)
+	wsr	a0, ps
 	bbsi.l	a0, PS_UM_BIT, 1f	# branch if user mode
 	rsr	a0, excsave2		# restore a0
 	j	_KernelExceptionVector	# simulate kernel vector exception