Add interrupt vector set/get affinity

Close #3071.

3 files changed, 52 insertions, 10 deletions

diff --git a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
index f2372ec363..2b0dbae8e7 100644
--- a/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
+++ b/c/src/lib/libbsp/sparc/leon3/clock/ckinit.c
@@ -192,16 +192,7 @@ static void bsp_clock_handler_install(rtems_isr *new)
 }
 
 #define Clock_driver_support_set_interrupt_affinity(online_processors) \
-  do { \
-    uint32_t cpu_count = _SMP_Processor_count; \
-    uint32_t cpu_index; \
-    LEON_Enable_interrupt_broadcast(clkirq); \
-    for (cpu_index = 0; cpu_index < cpu_count; ++cpu_index) { \
-      if (_Processor_mask_Is_set(online_processors, cpu_index)) { \
-        BSP_Cpu_Unmask_interrupt(clkirq, cpu_index); \
-      } \
-    } \
-  } while (0)
+  bsp_interrupt_set_affinity(clkirq, online_processors)
 
 static void leon3_clock_initialize(void)
 {
diff --git a/c/src/lib/libbsp/sparc/leon3/include/bsp/irq.h b/c/src/lib/libbsp/sparc/leon3/include/bsp/irq.h
index b429c864b5..964cc8c1e5 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/bsp/irq.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/bsp/irq.h
@@ -41,4 +41,14 @@ static inline bool bsp_interrupt_is_valid_vector(rtems_vector_number vector)
         LEON3_IrqCtrl_EIrq != 0));
 }
 
+void bsp_interrupt_set_affinity(
+  rtems_vector_number vector,
+  const Processor_mask *affinity
+);
+
+void bsp_interrupt_get_affinity(
+  rtems_vector_number vector,
+  Processor_mask *affinity
+);
+
 #endif /* LIBBSP_LEON3_IRQ_CONFIG_H */
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/eirq.c b/c/src/lib/libbsp/sparc/leon3/startup/eirq.c
index 7e8eb03762..1f7be1ba74 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/eirq.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/eirq.c
@@ -11,6 +11,7 @@
  */
 
 #include <leon.h>
+#include <bsp/irq.h>
 
 /* GRLIB extended IRQ controller IRQ number */
 int LEON3_IrqCtrl_EIrq = -1;
@@ -23,3 +24,43 @@ void leon3_ext_irq_init(void)
     LEON3_IrqCtrl_EIrq = (LEON3_IrqCtrl_Regs->mpstat >> 16) & 0xf;
   }
 }
+
+void bsp_interrupt_set_affinity(
+  rtems_vector_number vector,
+  const Processor_mask *affinity
+)
+{
+  uint32_t unmasked = 0;
+  uint32_t cpu_count = rtems_get_processor_count();
+  uint32_t cpu_index;
+
+  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);
+      ++unmasked;
+    }
+  }
+
+  if (unmasked > 1) {
+    LEON_Enable_interrupt_broadcast(vector);
+  } else {
+    LEON_Disable_interrupt_broadcast(vector);
+  }
+}
+
+void bsp_interrupt_get_affinity(
+  rtems_vector_number vector,
+  Processor_mask *affinity
+)
+{
+  uint32_t cpu_count = rtems_get_processor_count();
+  uint32_t cpu_index;
+
+  _Processor_mask_Zero(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);
+    }
+  }
+}