arm: Use Per_CPU_Control::isr_dispatch_disable

Update #2751.

4 files changed, 94 insertions, 31 deletions

diff --git a/cpukit/score/cpu/arm/arm_exc_interrupt.S b/cpukit/score/cpu/arm/arm_exc_interrupt.S
index de5a022fd7..fc02c91dca 100644
--- a/cpukit/score/cpu/arm/arm_exc_interrupt.S
+++ b/cpukit/score/cpu/arm/arm_exc_interrupt.S
@@ -43,7 +43,7 @@
 #define EXCHANGE_SIZE 16
 
 #define SELF_CPU_CONTROL r7
-#define SP_OF_INTERRUPTED_CONTEXT r9
+#define NON_VOLATILE_SCRATCH r9
 
 #define CONTEXT_LIST {r0, r1, r2, r3, EXCHANGE_LR, EXCHANGE_SPSR, SELF_CPU_CONTROL, r12}
 #define CONTEXT_SIZE 32
@@ -73,7 +73,7 @@ _ARMV4_Exception_interrupt:
 	 * interrupted context.
 	 */
 	stmdb	sp!, CONTEXT_LIST
-	stmdb	sp!, {SP_OF_INTERRUPTED_CONTEXT, lr}
+	stmdb	sp!, {NON_VOLATILE_SCRATCH, lr}
 
 #ifdef ARM_MULTILIB_VFP
 	/* Save VFP context */
@@ -98,7 +98,7 @@ _ARMV4_Exception_interrupt:
 	ldr	r2, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL]
 
 	/* Switch stack if necessary and save original stack pointer */
-	mov	SP_OF_INTERRUPTED_CONTEXT, sp
+	mov	NON_VOLATILE_SCRATCH, sp
 	cmp	r2, #0
 	moveq	sp, r1
 
@@ -130,33 +130,68 @@ _ARMV4_Exception_interrupt:
 	bl	bsp_interrupt_dispatch
 #endif
 
-	/* Decrement interrupt nest and thread dispatch disable level */
-	ldr	r2, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL]
-	ldr	r3, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
-	sub	r2, #1
-	sub	r3, #1
-	str	r2, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL]
-	str	r3, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
+	/* Load some per-CPU variables */
+	ldr	r0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
+	ldrb	r1, [SELF_CPU_CONTROL, #PER_CPU_DISPATCH_NEEDED]
+	ldr	r2, [SELF_CPU_CONTROL, #PER_CPU_ISR_DISPATCH_DISABLE]
+	ldr	r3, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL]
 
 	/* Restore stack pointer */
-	mov	sp, SP_OF_INTERRUPTED_CONTEXT
+	mov	sp, NON_VOLATILE_SCRATCH
 
-	/* Check thread dispatch disable level */
-	cmp	r3, #0
-	bne	.Lthread_dispatch_done
+	/* Save CPSR in non-volatile register */
+	mrs	NON_VOLATILE_SCRATCH, CPSR
 
-	/* Check context switch necessary */
-	ldrb	r1, [SELF_CPU_CONTROL, #PER_CPU_DISPATCH_NEEDED]
-	cmp	r1, #0
-	beq	.Lthread_dispatch_done
+	/* Decrement levels and determine thread dispatch state */
+	eor	r1, r0
+	sub	r0, #1
+	orr	r1, r0
+	orr	r1, r2
+	sub	r3, #1
 
-        /* This aligns .Lthread_dispatch_done on a 4 byte boundary */
-#ifdef __thumb__
-	nop
-#endif /* __thumb__ */
+	/* Store thread dispatch disable and ISR nest levels */
+	str	r0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
+	str	r3, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL]
+
+	/*
+	 * Check thread dispatch necessary, ISR dispatch disable and thread
+	 * dispatch disable level.
+	 */
+	cmp	r0, #0
+	bne	.Lthread_dispatch_done
 
 	/* Thread dispatch */
-	bl	_Thread_Dispatch
+	mrs	NON_VOLATILE_SCRATCH, CPSR
+
+.Ldo_thread_dispatch:
+
+	/* Set ISR dispatch disable and thread dispatch disable level to one */
+	mov	r0, #1
+	str	r0, [SELF_CPU_CONTROL, #PER_CPU_ISR_DISPATCH_DISABLE]
+	str	r0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
+
+	/* Call _Thread_Do_dispatch(), this function will enable interrupts */
+	mov	r0, SELF_CPU_CONTROL
+	mov	r1, NON_VOLATILE_SCRATCH
+	mov	r2, #0x80
+	bic	r1, r2
+	bl	_Thread_Do_dispatch
+
+	/* Disable interrupts */
+	msr	CPSR, NON_VOLATILE_SCRATCH
+
+#ifdef RTEMS_SMP
+	GET_SELF_CPU_CONTROL	SELF_CPU_CONTROL
+#endif
+
+	/* Check if we have to do the thread dispatch again */
+	ldrb	r0, [SELF_CPU_CONTROL, #PER_CPU_DISPATCH_NEEDED]
+	cmp	r0, #0
+	bne	.Ldo_thread_dispatch
+
+	/* We are done with thread dispatching */
+	mov	r0, #0
+	str	r0, [SELF_CPU_CONTROL, #PER_CPU_ISR_DISPATCH_DISABLE]
 
 .Lthread_dispatch_done:
 
@@ -173,8 +208,8 @@ _ARMV4_Exception_interrupt:
 	vmsr	FPSCR, r0
 #endif /* ARM_MULTILIB_VFP */
 
-	/* Restore SP_OF_INTERRUPTED_CONTEXT register and link register */
-	ldmia	sp!, {SP_OF_INTERRUPTED_CONTEXT, lr}
+	/* Restore NON_VOLATILE_SCRATCH register and link register */
+	ldmia	sp!, {NON_VOLATILE_SCRATCH, lr}
 
 	/*
 	 * XXX: Remember and restore stack pointer.  The data on the stack is
diff --git a/cpukit/score/cpu/arm/cpu.c b/cpukit/score/cpu/arm/cpu.c
index 944ca630c7..dc87844483 100644
--- a/cpukit/score/cpu/arm/cpu.c
+++ b/cpukit/score/cpu/arm/cpu.c
@@ -15,7 +15,7 @@
  *
  *  Copyright (c) 2007 Ray xu <rayx.cn@gmail.com>
  *
- *  Copyright (c) 2009-2011 embedded brains GmbH
+ *  Copyright (c) 2009, 2016 embedded brains GmbH
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -50,6 +50,14 @@
   );
 #endif
 
+#ifdef ARM_MULTILIB_ARCH_V4
+  RTEMS_STATIC_ASSERT(
+    offsetof( Context_Control, isr_dispatch_disable )
+      == ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE,
+    ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE
+  );
+#endif
+
 #ifdef RTEMS_SMP
   RTEMS_STATIC_ASSERT(
     offsetof( Context_Control, is_executing )
diff --git a/cpukit/score/cpu/arm/cpu_asm.S b/cpukit/score/cpu/arm/cpu_asm.S
index 1ad3a51b14..f10cd90ed8 100644
--- a/cpukit/score/cpu/arm/cpu_asm.S
+++ b/cpukit/score/cpu/arm/cpu_asm.S
@@ -19,7 +19,7 @@
  *  COPYRIGHT (c) 2000 Canon Research Centre France SA.
  *  Emmanuel Raguet, mailto:raguet@crf.canon.fr
  *
- *  Copyright (c) 2013-2015 embedded brains GmbH
+ *  Copyright (c) 2013, 2016 embedded brains GmbH
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -58,6 +58,9 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	mrs	r2, CPSR
 	stmia	r0,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
 
+	GET_SELF_CPU_CONTROL	r2
+	ldr	r4, [r2, #PER_CPU_ISR_DISPATCH_DISABLE]
+
 #ifdef ARM_MULTILIB_VFP
 	add	r3, r0, #ARM_CONTEXT_CONTROL_D8_OFFSET
 	vstm	r3, {d8-d15}
@@ -68,6 +71,8 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	str	r3, [r0, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
 #endif
 
+	str	r4, [r0, #ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE]
+
 #ifdef RTEMS_SMP
 	/*
 	 * The executing thread no longer executes on this processor.  Switch
@@ -75,7 +80,6 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	 * the context of the executing thread as not executing.
 	 */
 	dmb
-	GET_SELF_CPU_CONTROL	r2
 	add	sp, r2, #(PER_CPU_INTERRUPT_FRAME_AREA + CPU_INTERRUPT_FRAME_SIZE)
 	mov	r3, #0
 	strb	r3, [r0, #ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET]
@@ -102,6 +106,8 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	clrex
 #endif
 
+	ldr	r4, [r1, #ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE]
+
 #ifdef ARM_MULTILIB_HAS_THREAD_ID_REGISTER
 	ldr	r3, [r1, #ARM_CONTEXT_CONTROL_THREAD_ID_OFFSET]
 	mcr	p15, 0, r3, c13, c0, 3
@@ -112,6 +118,8 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
 	vldm	r3, {d8-d15}
 #endif
 
+	str	r4, [r2, #PER_CPU_ISR_DISPATCH_DISABLE]
+
 	ldmia	r1,  {r2, r4, r5, r6, r7, r8, r9, r10, r11, r13, r14}
 	msr	CPSR_fsxc, r2
 #ifdef __thumb__
@@ -129,6 +137,7 @@ DEFINE_FUNCTION_ARM(_CPU_Context_switch)
  */
 DEFINE_FUNCTION_ARM(_CPU_Context_restore)
         mov     r1, r0
+	GET_SELF_CPU_CONTROL	r2
         b       .L_restore
 
 #ifdef RTEMS_SMP
diff --git a/cpukit/score/cpu/arm/rtems/score/cpu.h b/cpukit/score/cpu/arm/rtems/score/cpu.h
index 326abbb662..b1fabafb59 100644
--- a/cpukit/score/cpu/arm/rtems/score/cpu.h
+++ b/cpukit/score/cpu/arm/rtems/score/cpu.h
@@ -8,7 +8,7 @@
  *  This include file contains information pertaining to the ARM
  *  processor.
  *
- *  Copyright (c) 2009-2015 embedded brains GmbH.
+ *  Copyright (c) 2009, 2016 embedded brains GmbH
  *
  *  Copyright (c) 2007 Ray Xu <Rayx.cn@gmail.com>
  *
@@ -209,11 +209,19 @@
   #define ARM_CONTEXT_CONTROL_D8_OFFSET 48
 #endif
 
+#ifdef ARM_MULTILIB_ARCH_V4
+  #ifdef ARM_MULTILIB_VFP
+    #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 112
+  #else
+    #define ARM_CONTEXT_CONTROL_ISR_DISPATCH_DISABLE 48
+  #endif
+#endif
+
 #ifdef RTEMS_SMP
   #ifdef ARM_MULTILIB_VFP
-    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 112
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 116
   #else
-    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 48
+    #define ARM_CONTEXT_CONTROL_IS_EXECUTING_OFFSET 52
   #endif
 #endif
 
@@ -277,6 +285,9 @@ typedef struct {
   uint64_t register_d14;
   uint64_t register_d15;
 #endif
+#ifdef ARM_MULTILIB_ARCH_V4
+  uint32_t isr_dispatch_disable;
+#endif
 #ifdef RTEMS_SMP
   volatile bool is_executing;
 #endif