bsps/powerpc: Per-CPU thread dispatch disable

Interrupt support for per-CPU thread dispatch disable level.

3 files changed, 45 insertions, 53 deletions

diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
index e3d747f870..0e3bc96895 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_asm_macros.h
@@ -14,6 +14,7 @@
 
 #include <bspopts.h>
 #include <bsp/vectors.h>
+#include <libcpu/powerpc-utility.h>
 
 #define LT(cr) ((cr)*4+0)
 #define GT(cr) ((cr)*4+1)
@@ -441,13 +442,13 @@ wrap_no_save_frame_register_\_FLVR:
 	 */
 
 	/* Increment ISR nest level and thread dispatch disable level */
-	lis	SCRATCH_REGISTER_2, ISR_NEST_LEVEL@ha
-	lwz	SCRATCH_REGISTER_0, ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
-	lwz	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	GET_SELF_CPU_CONTROL	SCRATCH_REGISTER_2
+	lwz	SCRATCH_REGISTER_0, PER_CPU_ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
+	lwz	SCRATCH_REGISTER_1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_REGISTER_2)
 	addi	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0, 1
 	addi	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, 1
-	stw	SCRATCH_REGISTER_0, ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
-	stw	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	stw	SCRATCH_REGISTER_0, PER_CPU_ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
+	stw	SCRATCH_REGISTER_1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_REGISTER_2)
 
 	/*
 	 * No higher-priority exception occurring after this point
@@ -636,13 +637,13 @@ wrap_handler_done_\_FLVR:
 	 */
 
 	/* Decrement ISR nest level and thread dispatch disable level */
-	lis	SCRATCH_REGISTER_2, ISR_NEST_LEVEL@ha
-	lwz	SCRATCH_REGISTER_0, ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
-	lwz	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	GET_SELF_CPU_CONTROL	SCRATCH_REGISTER_2
+	lwz	SCRATCH_REGISTER_0, PER_CPU_ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
+	lwz	SCRATCH_REGISTER_1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_REGISTER_2)
 	subi	SCRATCH_REGISTER_0, SCRATCH_REGISTER_0, 1
 	subic.	SCRATCH_REGISTER_1, SCRATCH_REGISTER_1, 1
-	stw	SCRATCH_REGISTER_0, ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
-	stw	SCRATCH_REGISTER_1, _Thread_Dispatch_disable_level@sdarel(r13)
+	stw	SCRATCH_REGISTER_0, PER_CPU_ISR_NEST_LEVEL@l(SCRATCH_REGISTER_2)
+	stw	SCRATCH_REGISTER_1, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SCRATCH_REGISTER_2)
 
 	/* Branch to skip thread dispatching */
 	bne	wrap_thread_dispatching_done_\_FLVR
diff --git a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
index 399c227ad3..7014530251 100644
--- a/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
+++ b/c/src/lib/libcpu/powerpc/new-exceptions/bspsupport/ppc_exc_async_normal.S
@@ -17,7 +17,7 @@
 #include <bsp/vectors.h>
 
 #define VECTOR_REGISTER r4
-#define ISR_NEST_HADDR_REGISTER r5
+#define SELF_CPU_REGISTER r5
 #define ISR_NEST_REGISTER r6
 #define DISPATCH_LEVEL_REGISTER r7
 #define HANDLER_REGISTER r8
@@ -30,7 +30,7 @@
 #define FRAME_REGISTER r14
 
 #define VECTOR_OFFSET(reg) GPR4_OFFSET(reg)
-#define ISR_NEST_HADDR_OFFSET(reg) GPR5_OFFSET(reg)
+#define SELF_CPU_OFFSET(reg) GPR5_OFFSET(reg)
 #define ISR_NEST_OFFSET(reg) GPR6_OFFSET(reg)
 #define DISPATCH_LEVEL_OFFSET(reg) GPR7_OFFSET(reg)
 #define HANDLER_OFFSET(reg) GPR8_OFFSET(reg)
@@ -85,12 +85,12 @@ ppc_exc_wrap_async_normal:
 	mr	FRAME_REGISTER, r1
 
 	/* Load ISR nest level and thread dispatch disable level */
-	PPC_GPR_STORE	ISR_NEST_HADDR_REGISTER, ISR_NEST_HADDR_OFFSET(r1)
-	lis	ISR_NEST_HADDR_REGISTER, ISR_NEST_LEVEL@ha
+	PPC_GPR_STORE	SELF_CPU_REGISTER, SELF_CPU_OFFSET(r1)
+	GET_SELF_CPU_CONTROL	SELF_CPU_REGISTER
 	PPC_GPR_STORE	ISR_NEST_REGISTER, ISR_NEST_OFFSET(r1)
-	lwz	ISR_NEST_REGISTER, ISR_NEST_LEVEL@l(ISR_NEST_HADDR_REGISTER)
+	lwz	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
 	PPC_GPR_STORE	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_OFFSET(r1)
-	lwz	DISPATCH_LEVEL_REGISTER, _Thread_Dispatch_disable_level@sdarel(r13)
+	lwz	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
 
 	PPC_GPR_STORE	SCRATCH_0_REGISTER, SCRATCH_0_OFFSET(r1)
 
@@ -152,13 +152,12 @@ ppc_exc_wrap_async_normal:
 	evstdd	SCRATCH_1_REGISTER, PPC_EXC_ACC_OFFSET(r1)
 #endif
 
-#ifndef RTEMS_SMP
 	/* Increment ISR nest level and thread dispatch disable level */
 	cmpwi	ISR_NEST_REGISTER, 0
 	addi	ISR_NEST_REGISTER, ISR_NEST_REGISTER, 1
 	addi	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_REGISTER, 1
-	stw	ISR_NEST_REGISTER, ISR_NEST_LEVEL@l(ISR_NEST_HADDR_REGISTER)
-	stw	DISPATCH_LEVEL_REGISTER, _Thread_Dispatch_disable_level@sdarel(r13)
+	stw	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
+	stw	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
 
 	/* Switch stack if necessary */
 	mfspr	SCRATCH_0_REGISTER, SPRG1
@@ -181,9 +180,9 @@ ppc_exc_wrap_async_normal:
 #endif /* PPC_EXC_CONFIG_USE_FIXED_HANDLER */
 
 	/* Load ISR nest level and thread dispatch disable level */
-	lis	ISR_NEST_HADDR_REGISTER, ISR_NEST_LEVEL@ha
-	lwz	ISR_NEST_REGISTER, ISR_NEST_LEVEL@l(ISR_NEST_HADDR_REGISTER)
-	lwz	DISPATCH_LEVEL_REGISTER, _Thread_Dispatch_disable_level@sdarel(r13)
+	GET_SELF_CPU_CONTROL	SELF_CPU_REGISTER
+	lwz	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
+	lwz	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
 
 	/*
 	 * Switch back to original stack (FRAME_REGISTER == r1 if we are still
@@ -195,30 +194,8 @@ ppc_exc_wrap_async_normal:
 	/* Decrement ISR nest level and thread dispatch disable level */
 	subi	ISR_NEST_REGISTER, ISR_NEST_REGISTER, 1
 	subic.	DISPATCH_LEVEL_REGISTER, DISPATCH_LEVEL_REGISTER, 1
-	stw	ISR_NEST_REGISTER, ISR_NEST_LEVEL@l(ISR_NEST_HADDR_REGISTER)
-	stw	DISPATCH_LEVEL_REGISTER, _Thread_Dispatch_disable_level@sdarel(r13)
-#else /* RTEMS_SMP */
-	/* ISR Enter */
-	bl	_ISR_SMP_Enter
-	cmpwi	r3, 0
-
-	/* Switch stack if necessary */
-	mfspr	SCRATCH_0_REGISTER, SPRG1
-	iselgt	r1, r1, SCRATCH_0_REGISTER
-
-	bl      bsp_interrupt_dispatch
-
-	/*
-	 * Switch back to original stack (FRAME_REGISTER == r1 if we are still
-	 * on the IRQ stack) and restore FRAME_REGISTER.
-	 */
-	mr	r1, FRAME_REGISTER
-	lwz	FRAME_REGISTER, FRAME_OFFSET(r1)
-
-	/* ISR Leave */
-	bl	_ISR_SMP_Exit
-	cmpwi	r3, 1
-#endif /* RTEMS_SMP */
+	stw	ISR_NEST_REGISTER, PER_CPU_ISR_NEST_LEVEL(SELF_CPU_REGISTER)
+	stw	DISPATCH_LEVEL_REGISTER, PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL(SELF_CPU_REGISTER)
 
 	/* Call thread dispatcher if necessary */
 	bne	thread_dispatching_done
@@ -240,7 +217,7 @@ thread_dispatching_done:
 	lwz	SCRATCH_5_REGISTER, EXC_LR_OFFSET(r1)
 
 	PPC_GPR_LOAD	VECTOR_REGISTER, VECTOR_OFFSET(r1)
-	PPC_GPR_LOAD	ISR_NEST_HADDR_REGISTER, ISR_NEST_HADDR_OFFSET(r1)
+	PPC_GPR_LOAD	SELF_CPU_REGISTER, SELF_CPU_OFFSET(r1)
 	PPC_GPR_LOAD	ISR_NEST_REGISTER, ISR_NEST_OFFSET(r1)
 
 #ifdef __SPE__
diff --git a/c/src/lib/libcpu/powerpc/shared/include/powerpc-utility.h b/c/src/lib/libcpu/powerpc/shared/include/powerpc-utility.h
index 71567d5814..3994255080 100644
--- a/c/src/lib/libcpu/powerpc/shared/include/powerpc-utility.h
+++ b/c/src/lib/libcpu/powerpc/shared/include/powerpc-utility.h
@@ -8,12 +8,13 @@
  */
 
 /*
- * Copyright (c) 2008, 2010, 2011
- * Embedded Brains GmbH
- * Obere Lagerstr. 30
- * D-82178 Puchheim
- * Germany
- * rtems@embedded-brains.de
+ * Copyright (c) 2008-2013 embedded brains GmbH.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
  *
  * access function for Device Control Registers inspired by "ppc405common.h"
  * from Michael Hamel ADInstruments May 2008
@@ -893,6 +894,19 @@ void ppc_code_copy(void *dest, const void *src, size_t n);
 	mtmsr	\level
 .endm
 
+.macro GET_SELF_CPU_CONTROL reg
+#if defined(RTEMS_SMP)
+	/* Use Book E Processor ID Register (PIR) */
+	mfspr	\reg, 286
+	slwi	\reg, \reg, PER_CPU_CONTROL_SIZE_LOG2
+	addis	\reg, \reg, _Per_CPU_Information@ha
+	addi	\reg, \reg, _Per_CPU_Information@l
+#else
+	lis	\reg, _Per_CPU_Information@h
+	ori	\reg, \reg, _Per_CPU_Information@l
+#endif
+.endm
+
 #define LINKER_SYMBOL(sym) .extern sym
 
 #endif /* ASM */