bsps/sparc: Improve interrupt affinity support

Fully support the interrupt extension API to set/get the interrupt affinity.
Remove LEON3_irq_to_cpu which defined the interrupt to processor mapping in a
BSP-specific way.

Update #3269.

4 files changed, 138 insertions, 62 deletions

diff --git a/bsps/sparc/leon3/include/bsp.h b/bsps/sparc/leon3/include/bsp.h
index cb08764eae..850220d70d 100644
--- a/bsps/sparc/leon3/include/bsp.h
+++ b/bsps/sparc/leon3/include/bsp.h
@@ -214,17 +214,6 @@ extern void BSP_shared_interrupt_mask(int irq);
 extern const unsigned char LEON3_mp_irq;
 #endif
 
-#ifdef RTEMS_SMP
-/* Weak table used to implement static interrupt CPU affinity in a SMP
- * configuration. The array index is the interrupt to be looked up, and
- * the array[INTERRUPT] content is the CPU number relative to boot CPU
- * index that will be servicing the interrupts from the IRQ source. The
- * default is to let the first CPU (the boot cpu) to handle all
- * interrupts (all zeros).
- */
-extern const unsigned char LEON3_irq_to_cpu[32];
-#endif
-
 /* Common driver build-time configurations. On small systems undefine
  * [DRIVER]_INFO_AVAIL to avoid info routines get dragged in. It is good
  * for debugging and printing information about the system, but makes the
diff --git a/bsps/sparc/leon3/start/bspsmp.c b/bsps/sparc/leon3/start/bspsmp.c
index f2749bfa06..1c72027ebf 100644
--- a/bsps/sparc/leon3/start/bspsmp.c
+++ b/bsps/sparc/leon3/start/bspsmp.c
@@ -16,6 +16,7 @@
 #include <bsp.h>
 #include <bsp/bootcard.h>
 #include <bsp/fatal.h>
+#include <bsp/irq.h>
 #include <leon.h>
 #include <rtems/bspIo.h>
 #include <rtems/sysinit.h>
@@ -38,8 +39,6 @@ static void bsp_inter_processor_interrupt( void *arg )
 
 void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
 {
-  uint32_t cpu_index_self;
-
   /*
    * If data cache snooping is not enabled we terminate using BSP_fatal_exit()
    * instead of bsp_fatal().  This is done since the latter function tries to
@@ -49,19 +48,20 @@ void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
   if ( !leon3_data_cache_snooping_enabled() )
     BSP_fatal_exit( LEON3_FATAL_INVALID_CACHE_CONFIG_SECONDARY_PROCESSOR );
 
-  /* Unmask IPI interrupts at Interrupt controller for this CPU */
-  cpu_index_self = _Per_CPU_Get_index(cpu_self);
-  LEON3_IrqCtrl_Regs->mask[cpu_index_self] |= 1U << LEON3_mp_irq;
-
   _SMP_Start_multitasking_on_secondary_processor(cpu_self);
 }
 
 static void leon3_install_inter_processor_interrupt( void )
 {
   rtems_status_code sc;
+  rtems_vector_number irq;
+
+  irq = LEON3_mp_irq;
+
+  bsp_interrupt_set_affinity( irq, _SMP_Get_online_processors() );
 
   sc = rtems_interrupt_handler_install(
-    LEON3_mp_irq,
+    irq,
     "IPI",
     RTEMS_INTERRUPT_SHARED,
     bsp_inter_processor_interrupt,
@@ -75,10 +75,6 @@ uint32_t _CPU_SMP_Initialize( void )
   if ( !leon3_data_cache_snooping_enabled() )
     bsp_fatal( LEON3_FATAL_INVALID_CACHE_CONFIG_MAIN_PROCESSOR );
 
-#if !defined(RTEMS_DRVMGR_STARTUP)
-  leon3_install_inter_processor_interrupt();
-#endif
-
   return leon3_get_cpu_count(LEON3_IrqCtrl_Regs);
 }
 
@@ -97,7 +93,9 @@ void _CPU_SMP_Finalize_initialization( uint32_t cpu_count )
 {
   (void) cpu_count;
 
-  /* Nothing to do */
+#if !defined(RTEMS_DRVMGR_STARTUP)
+  leon3_install_inter_processor_interrupt();
+#endif
 }
 
 void _CPU_SMP_Prepare_start_multitasking( void )
diff --git a/bsps/sparc/leon3/start/eirq.c b/bsps/sparc/leon3/start/eirq.c
index e3ae5b1a4d..84e81a90a4 100644
--- a/bsps/sparc/leon3/start/eirq.c
+++ b/bsps/sparc/leon3/start/eirq.c
@@ -1,6 +1,8 @@
 /*
  *  GRLIB/LEON3 extended interrupt controller
  *
+ *  Copyright (C) 2021 embedded brains GmbH (http://www.embedded-brains.de)
+ *
  *  COPYRIGHT (c) 2011
  *  Aeroflex Gaisler
  *
@@ -39,42 +41,141 @@ bool bsp_interrupt_is_valid_vector(rtems_vector_number vector)
   return vector <= BSP_INTERRUPT_VECTOR_MAX_STD;
 }
 
-void bsp_interrupt_set_affinity(
-  rtems_vector_number vector,
-  const Processor_mask *affinity
-)
+#if defined(RTEMS_SMP)
+Processor_mask leon3_interrupt_affinities[BSP_INTERRUPT_VECTOR_MAX_STD + 1];
+#endif
+
+rtems_status_code bsp_interrupt_facility_initialize(void)
+{
+#if defined(RTEMS_SMP)
+  Processor_mask affinity;
+  size_t i;
+
+  _Processor_mask_From_index(&affinity, rtems_scheduler_get_processor());
+
+  for (i = 0; i < RTEMS_ARRAY_SIZE(leon3_interrupt_affinities); ++i) {
+    leon3_interrupt_affinities[i] = affinity;
+  }
+#endif
+
+  return RTEMS_SUCCESSFUL;
+}
+
+#if defined(RTEMS_SMP)
+static void leon3_interrupt_vector_enable(rtems_vector_number vector)
 {
-  uint32_t unmasked = 0;
-  uint32_t cpu_count = rtems_scheduler_get_processor_maximum();
   uint32_t cpu_index;
+  uint32_t cpu_count;
+  Processor_mask affinity;
+  uint32_t bit;
+  uint32_t unmasked;
+
+  cpu_count = rtems_scheduler_get_processor_maximum();
+  affinity = leon3_interrupt_affinities[vector];
+  bit = 1U << vector;
+  unmasked = 0;
 
   for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) {
-    if (_Processor_mask_Is_set(affinity, cpu_index)) {
-      BSP_Cpu_Unmask_interrupt(vector, cpu_index);
+    uint32_t mask;
+
+    mask = LEON3_IrqCtrl_Regs->mask[cpu_index];
+
+    if (_Processor_mask_Is_set(&affinity, cpu_index)) {
       ++unmasked;
+      mask |= bit;
+    } else {
+      mask &= ~bit;
     }
+
+    LEON3_IrqCtrl_Regs->mask[cpu_index] = mask;
   }
 
   if (unmasked > 1) {
-    LEON_Enable_interrupt_broadcast(vector);
+    LEON3_IrqCtrl_Regs->bcast |= bit;
   } else {
-    LEON_Disable_interrupt_broadcast(vector);
+    LEON3_IrqCtrl_Regs->bcast &= ~bit;
   }
 }
+#endif
 
-void bsp_interrupt_get_affinity(
+void bsp_interrupt_vector_enable(rtems_vector_number vector)
+{
+#if defined(RTEMS_SMP)
+  rtems_interrupt_lock_context lock_context;
+
+  bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
+  LEON3_IRQCTRL_ACQUIRE(&lock_context);
+  leon3_interrupt_vector_enable(vector);
+  LEON3_IRQCTRL_RELEASE(&lock_context);
+#else
+  bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
+  BSP_Cpu_Unmask_interrupt(vector, _LEON3_Get_current_processor());
+#endif
+}
+
+void bsp_interrupt_vector_disable(rtems_vector_number vector)
+{
+#if defined(RTEMS_SMP)
+  rtems_interrupt_lock_context lock_context;
+  uint32_t bit;
+  uint32_t cpu_index;
+  uint32_t cpu_count;
+
+  bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
+  bit = 1U << vector;
+  cpu_count = rtems_scheduler_get_processor_maximum();
+
+  LEON3_IRQCTRL_ACQUIRE(&lock_context);
+
+  for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) {
+    LEON3_IrqCtrl_Regs->mask[cpu_index] &= ~bit;
+  }
+
+  LEON3_IrqCtrl_Regs->bcast &= ~bit;
+
+  LEON3_IRQCTRL_RELEASE(&lock_context);
+#else
+  bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
+  BSP_Cpu_Mask_interrupt(vector, _LEON3_Get_current_processor());
+#endif
+}
+
+#if defined(RTEMS_SMP)
+void bsp_interrupt_set_affinity(
   rtems_vector_number vector,
-  Processor_mask *affinity
+  const Processor_mask *affinity
 )
 {
-  uint32_t cpu_count = rtems_scheduler_get_processor_maximum();
+  rtems_interrupt_lock_context lock_context;
+  uint32_t cpu_count;
   uint32_t cpu_index;
+  uint32_t bit;
+
+  cpu_count = rtems_scheduler_get_processor_maximum();
+  bit = 1U << vector;
 
-  _Processor_mask_Zero(affinity);
+  LEON3_IRQCTRL_ACQUIRE(&lock_context);
+  leon3_interrupt_affinities[vector] = *affinity;
 
+  /*
+   * If the interrupt is enabled on at least one processor, then re-enable it
+   * using the new affinity.
+   */
   for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) {
-    if (!BSP_Cpu_Is_interrupt_masked(vector, cpu_index)) {
-      _Processor_mask_Set(affinity, cpu_index);
+    if ((LEON3_IrqCtrl_Regs->mask[cpu_index] & bit) != 0) {
+      leon3_interrupt_vector_enable(vector);
+      break;
     }
   }
+
+  LEON3_IRQCTRL_RELEASE(&lock_context);
+}
+
+void bsp_interrupt_get_affinity(
+  rtems_vector_number vector,
+  Processor_mask *affinity
+)
+{
+  *affinity = leon3_interrupt_affinities[vector];
 }
+#endif
diff --git a/bsps/sparc/shared/irq/irq-shared.c b/bsps/sparc/shared/irq/irq-shared.c
index a127ab64c2..42e5b010fb 100644
--- a/bsps/sparc/shared/irq/irq-shared.c
+++ b/bsps/sparc/shared/irq/irq-shared.c
@@ -8,54 +8,42 @@
 *
 */
 
-#include <rtems.h>
 #include <bsp.h>
 #include <bsp/irq-generic.h>
 
-#if defined(RTEMS_SMP) && defined(LEON3)
-/* Interrupt to CPU map. Default to CPU0 since in BSS. */
-const unsigned char LEON3_irq_to_cpu[32] __attribute__((weak));
-
-/* On SMP use map table above relative to SMP Boot CPU (normally CPU0) */
-static inline int bsp_irq_cpu(int irq)
-{
-  /* protect from bad user configuration, default to boot cpu */
-  if (rtems_configuration_get_maximum_processors() <= LEON3_irq_to_cpu[irq])
-    return LEON3_Cpu_Index;
-  else
-    return LEON3_Cpu_Index + LEON3_irq_to_cpu[irq];
-}
-#else
-/* when not SMP the local CPU is returned */
 static inline int bsp_irq_cpu(int irq)
 {
-#ifdef LEON3
+#if defined(RTEMS_SMP)
+  Processor_mask affinity;
+
+  bsp_interrupt_get_affinity((rtems_vector_number) irq, &affinity);
+  return (int) _Processor_mask_Find_last_set(&affinity);
+#elif defined(LEON3)
   return _LEON3_Get_current_processor();
 #else
   return 0;
 #endif
 }
-#endif
 
-/* Callback from bsp_interrupt_initialize() */
+#if !defined(LEON3)
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
+  /* Nothing to do */
   return RTEMS_SUCCESSFUL;
 }
 
 void bsp_interrupt_vector_enable(rtems_vector_number vector)
 {
-  int irq = (int)vector;
   bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
-  BSP_Cpu_Unmask_interrupt(irq, bsp_irq_cpu(irq));
+  BSP_Cpu_Unmask_interrupt(vector, 0);
 }
 
 void bsp_interrupt_vector_disable(rtems_vector_number vector)
 {
-  int irq = (int)vector;
   bsp_interrupt_assert(bsp_interrupt_is_valid_vector(vector));
-  BSP_Cpu_Mask_interrupt(irq, bsp_irq_cpu(irq));
+  BSP_Cpu_Mask_interrupt(vector, 0);
 }
+#endif
 
 void BSP_shared_interrupt_mask(int irq)
 {