nios2: Optimize ISR dispatch variant

Use _Thread_Do_dispatch() in
_Nios2_ISR_Dispatch_with_shadow_non_preemptive().

1 files changed, 60 insertions, 42 deletions

diff --git a/cpukit/score/cpu/nios2/nios2-eic-il-low-level.S b/cpukit/score/cpu/nios2/nios2-eic-il-low-level.S
index c215d4bd94..1632fbb8c2 100644
--- a/cpukit/score/cpu/nios2/nios2-eic-il-low-level.S
+++ b/cpukit/score/cpu/nios2/nios2-eic-il-low-level.S
@@ -55,16 +55,16 @@ _Nios2_ISR_Dispatch_with_shadow_non_preemptive:
 	ldw	r4, 8(et)
 
 	/* Increment and store thread dispatch disable level */
-	addi	r9, r16, 1
-	stw	r9, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp)
+	addi	r17, r16, 1
+	stw	r17, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp)
 
 	/* Call high level handler with argument */
 	callr	r8
 
-	/* Load thread dispatch necessary */
+	/* Load the thread dispatch necessary indicator */
 	ldb	r12, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp)
 
-	/* Load thread dispatch after ISR disable indicator */
+	/* Load the thread dispatch after ISR disable indicator */
 	ldw	r13, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp)
 
 	/* Read status */
@@ -74,31 +74,36 @@ _Nios2_ISR_Dispatch_with_shadow_non_preemptive:
 	subi	ea, ea, 4
 
 	/*
-	 * Restore the thread dispatch disable level.  We must do this before
-	 * we return to the normal register set, because otherwise we have
-	 * problems if someone deletes or restarts the interrupted thread while
-	 * we are in the thread dispatch helper.
+	 * If the current thread dispatch disable level (r17) is one, then
+	 * negate the thread dispatch necessary indicator, otherwise the value
+	 * is irrelevant.  Or it with the previous thread dispatch disable
+	 * level value (r16).  The r15 which will be used as a status to
+	 * determine if a thread dispatch is necessary and allowed.
 	 */
-	stw	r16, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp)
-
-	/* Is thread dispatch allowed? */
-	bne	r16, zero, no_thread_dispatch
+	xor	r12, r17, r12
+	or	r15, r12, r16
 
-	/* Is thread dispatch necessary? */
-	beq	r12, zero, no_thread_dispatch
+	/*
+	 * Get the previous register set from r14.  If it is zero, then this is
+	 * the outermost interrupt.  Or it to the thread dispatch status (r15).
+	 */
+	andhi	r12, r14, 0x3f
+	or	r15, r12, r15
 
-	/* Is outermost interrupt? */
-	andhi	r14, r14, 0x3f
-	bne	r14, zero, no_thread_dispatch
+	/*
+	 * Or the thread dispatch after ISR disable indicator (r13) to the
+	 * thread dispatch status (r15).
+	 */
+	or	r15, r13, r15
 
-	/* Is thread dispatch after ISR allowed? */
-	bne	r13, zero, no_thread_dispatch
+	/* Is a thread dispatch necessary and allowed? */
+	bne	r15, zero, no_thread_dispatch
 
 	/* Obtain stack frame in normal register set */
 	rdprs	r15, sp, -FRAME_SIZE
 
 	/* Disable thread dispatch after ISR */
-	stw	r12, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp)
+	stw	r17, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp)
 
 	/* Save context */
 	stw	sstatus, FRAME_OFFSET_STATUS(r15)
@@ -111,12 +116,15 @@ _Nios2_ISR_Dispatch_with_shadow_non_preemptive:
 	/* Update stack pointer in normal register set */
 	wrprs	sp, r15
 
+	/* Jump to thread dispatch helper */
+	eret
+
 no_thread_dispatch:
 
-	/*
-	 * Return to thread dispatch helper, interrupted thread or interrupted
-	 * lower level interrupt service routine.
-	 */
+	/* Restore the thread dispatch disable level */
+	stw	r16, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp)
+
+	/* Return to interrupted context */
 	eret
 
 thread_dispatch_helper:
@@ -141,9 +149,26 @@ thread_dispatch_helper:
 	stw	r14, FRAME_OFFSET_R14(sp)
 	stw	r15, FRAME_OFFSET_R15(sp)
 
+	/*
+	 * Disable interrupts (1).
+	 *
+	 * We have the following invariants:
+	 *   1. status.RSIE == 0: thread context initialization
+	 *   2. status.CRS == 0: thread context initialization
+	 *   3. status.PRS: arbitrary
+	 *   4. status.IL < interrupt disable IL: else we would not be here
+	 *   5. status.IH == 0: thread context initialization
+	 *   6. status.U == 0: thread context initialization
+	 *   7. status.PIE == 1: thread context initialization
+	 * Thus we can use a constant to disable interrupts.
+	 */
+	movi	r5, %lo(_Nios2_ISR_Status_interrupts_disabled)
+	wrctl	status, r5
+
 do_thread_dispatch:
 
-	call	_Thread_Dispatch
+	addi	r4, gp, %gprel(_Per_CPU_Information)
+	call	_Thread_Do_dispatch
 
 	/* Restore some volatile registers */
 	ldw	ra, FRAME_OFFSET_RA(sp)
@@ -160,19 +185,7 @@ do_thread_dispatch:
 	ldw	r11, FRAME_OFFSET_R11(sp)
 	ldw	r12, FRAME_OFFSET_R12(sp)
 
-	/*
-	 * Disable interrupts.
-	 *
-	 * We have the following invariants:
-	 *   1. status.RSIE == 0: thread context initialization
-	 *   2. status.CRS == 0: thread context initialization
-	 *   3. status.PRS: arbitrary
-	 *   4. status.IL < interrupt disable IL: else we would not be here
-	 *   5. status.IH == 0: thread context initialization
-	 *   6. status.U == 0: thread context initialization
-	 *   7. status.PIE == 1: thread context initialization
-	 * Thus we can use a constant to disable interrupts.
-	 */
+	/* Disable interrupts, see (1) */
 	rdctl	r14, status
 	movi	r15, %lo(_Nios2_ISR_Status_interrupts_disabled)
 	wrctl	status, r15
@@ -181,7 +194,7 @@ do_thread_dispatch:
 	ldb	r13, %gprel(_Per_CPU_Information + PER_CPU_DISPATCH_NEEDED)(gp)
 
 	/* Is thread dispatch necessary? */
-	bne	r13, zero, enable_interrupts_before_thread_dispatch
+	bne	r13, zero, prepare_thread_dispatch
 
 	/* Enable thread dispatch after ISR */
 	stw	zero, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp)
@@ -204,9 +217,14 @@ do_thread_dispatch:
 	/* Return to interrupted thread */
 	eret
 
-enable_interrupts_before_thread_dispatch:
+prepare_thread_dispatch:
+
+	/* Disable thread dispatching */
+	movi	r4, 1
+	stw	r4, %gprel(_Per_CPU_Information + PER_CPU_ISR_DISPATCH_DISABLE)(gp)
+	stw	r4, %gprel(_Per_CPU_Information + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL)(gp)
 
-	/* Restore status */
-	wrctl	status, r14
+	/* Set interrupt level argument for _Thread_Do_dispatch() */
+	mov	r5, r15
 
 	br	do_thread_dispatch