[PATCH 08/12] bsps/sparc: Per-CPU thread dispatch disable

[Sebastian Huber]

Interrupt support for per-CPU thread dispatch disable level.
---
 c/src/lib/libbsp/sparc/shared/irq_asm.S |   27 ++++++++++++++++++++++-----
 1 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/shared/irq_asm.S b/c/src/lib/libbsp/sparc/shared/irq_asm.S
index f025005..f28046d 100644
--- a/c/src/lib/libbsp/sparc/shared/irq_asm.S
+++ b/c/src/lib/libbsp/sparc/shared/irq_asm.S
@@ -21,6 +21,25 @@
 
 #include <rtems/asm.h>
 #include <rtems/system.h>
+#include <bspopts.h>
+
+.macro GET_SELF_CPU_CONTROL REG, TMP
+        sethi    %hi(_Per_CPU_Information), \REG
+        add      \REG, %lo(_Per_CPU_Information), \REG
+
+#if defined( RTEMS_SMP )
+#if BSP_LEON3_SMP
+        /* LEON3 SMP support */
+        rd       %asr17, \TMP
+        srl      \TMP, 28, \TMP /* CPU number is upper 4 bits so shift */
+#else
+        mov      0, \TMP
+        nop
+#endif
+        sll      \TMP, PER_CPU_CONTROL_SIZE_LOG2, \TMP
+        add      \REG, \TMP, \REG
+#endif /* defined( RTEMS_SMP ) */
+.endm
 
 /*
  *  void _ISR_Handler()
@@ -174,8 +193,7 @@ save_isf:
          *        nest and thread dispatch disable levels are unnested.
          */
 
-        sethi    %hi(_Per_CPU_Information), %l5
-        add      %l5, %lo(_Per_CPU_Information), %l5
+	GET_SELF_CPU_CONTROL %l5, %l7
 
 	ld       [%l5 + PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL], %l6
 	ld       [%l5 + PER_CPU_ISR_NEST_LEVEL], %l7
@@ -332,7 +350,7 @@ dont_fix_pil2:
          *  Register usage for this section:
          *
          *    l4 = _Thread_Dispatch_disable_level pointer
-         *    l5 = _ISR_Nest_level pointer
+         *    l5 = per cpu info pointer
          *    l6 = _Thread_Dispatch_disable_level value
          *    l7 = _ISR_Nest_level value
          */
@@ -416,8 +434,7 @@ isr_dispatch:
          *  _Thread_Dispatch before leaving this ISR Dispatch context.
          */
 
-        sethi    %hi(_Per_CPU_Information), %l5
-        add      %l5, %lo(_Per_CPU_Information), %l5
+	GET_SELF_CPU_CONTROL %l5, %l7
 
         ldub     [%l5 + PER_CPU_DISPATCH_NEEDED], %l7
 
-- 
1.7.7