Add interrupt vector set/get affinity

Close #3071.

5 files changed, 74 insertions, 28 deletions

diff --git a/c/src/lib/libbsp/sparc/erc32/include/bsp/irq.h b/c/src/lib/libbsp/sparc/erc32/include/bsp/irq.h
index e0bc3393d8..9860a7e19b 100644
--- a/c/src/lib/libbsp/sparc/erc32/include/bsp/irq.h
+++ b/c/src/lib/libbsp/sparc/erc32/include/bsp/irq.h
@@ -18,6 +18,8 @@
 #ifndef LIBBSP_ERC32_IRQ_CONFIG_H
 #define LIBBSP_ERC32_IRQ_CONFIG_H
 
+#include <rtems.h>
+
 #define BSP_INTERRUPT_VECTOR_MAX_STD 15 /* Standard IRQ controller */
 #define BSP_INTERRUPT_VECTOR_MIN 0
 #define BSP_INTERRUPT_VECTOR_MAX BSP_INTERRUPT_VECTOR_MAX_STD
@@ -25,4 +27,22 @@
 /* No extra check is needed */
 #undef BSP_INTERRUPT_CUSTOM_VALID_VECTOR
 
+RTEMS_INLINE_ROUTINE void bsp_interrupt_set_affinity(
+  rtems_vector_number   vector,
+  const Processor_mask *affinity
+)
+{
+  (void) vector;
+  (void) affinity;
+}
+
+RTEMS_INLINE_ROUTINE void bsp_interrupt_get_affinity(
+  rtems_vector_number  vector,
+  Processor_mask      *affinity
+)
+{
+  (void) vector;
+  _Processor_mask_From_index( affinity, 0 );
+}
+
 #endif /* LIBBSP_ERC32_IRQ_CONFIG_H */
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);
+    }
+  }
+}
diff --git a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
index f61d2a8ab0..e299b10e96 100644
--- a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
+++ b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
@@ -22,6 +22,7 @@
 #include <bsp/genirq.h>
 
 #include <bsp.h>
+#include <bsp/irq.h>
 
 #define DBG(args...)
 /*#define DBG(args...) printk(args)*/
@@ -236,24 +237,7 @@ int ambapp_grlib_int_set_affinity
 	const Processor_mask *cpus
 	)
 {
-	uint32_t cpu_count = rtems_get_processor_count();
-	uint32_t cpu_index;
-	int enabled_cnt = 0;
-
-	for (cpu_index = 0; cpu_index < cpu_count; cpu_index++) {
-		if (_Processor_mask_Is_set(cpus, cpu_index)) {
-			BSP_Cpu_Unmask_interrupt(irq, cpu_index);
-			enabled_cnt++;
-		}
-	}
-
-	/* Propagate the interrupt to all CPUs */
-	if (enabled_cnt > 1) {
-		LEON_Enable_interrupt_broadcast(irq);
-	} else {
-		LEON_Disable_interrupt_broadcast(irq);
-	}
-
+	bsp_interrupt_set_affinity(irq, cpus);
 	return DRVMGR_OK;
 }
 #endif