bsps: Add GICv3 arm_gic_irq_processor_count()

Update #4202.

2 files changed, 34 insertions, 0 deletions

diff --git a/bsps/shared/dev/irq/arm-gicv2.c b/bsps/shared/dev/irq/arm-gicv2.c
index bd614bc1d8..97f397dffd 100644
--- a/bsps/shared/dev/irq/arm-gicv2.c
+++ b/bsps/shared/dev/irq/arm-gicv2.c
@@ -269,3 +269,10 @@ void arm_gic_trigger_sgi(rtems_vector_number vector, uint32_t targets)
 #endif
     | GIC_DIST_ICDSGIR_SGIINTID(vector);
 }
+
+uint32_t arm_gic_irq_processor_count(void)
+{
+  volatile gic_dist *dist = ARM_GIC_DIST;
+
+  return GIC_DIST_ICDICTR_CPU_NUMBER_GET(dist->icdictr) + 1;
+}
diff --git a/bsps/shared/dev/irq/arm-gicv3.c b/bsps/shared/dev/irq/arm-gicv3.c
index 536abdfc35..e23778ea37 100644
--- a/bsps/shared/dev/irq/arm-gicv3.c
+++ b/bsps/shared/dev/irq/arm-gicv3.c
@@ -354,3 +354,30 @@ void arm_gic_trigger_sgi(rtems_vector_number vector, uint32_t targets)
 #endif
   WRITE64_SR(ICC_SGI1, value);
 }
+
+uint32_t arm_gic_irq_processor_count(void)
+{
+  volatile gic_dist *dist = ARM_GIC_DIST;
+  uint32_t cpu_count;
+
+  if ((dist->icddcr & GIC_DIST_ICDDCR_ARE_S) == 0) {
+    cpu_count = GIC_DIST_ICDICTR_CPU_NUMBER_GET(dist->icdictr) + 1;
+  } else {
+    int i;
+
+    /* Assume that an interrupt export port exists */
+    cpu_count = 0;
+
+    for (i = 0; i < CPU_MAXIMUM_PROCESSORS; ++i) {
+      volatile gic_redist *redist = gicv3_get_redist(i);
+
+      if ((redist->icrtyper & GIC_REDIST_ICRTYPER_LAST) != 0) {
+        break;
+      }
+
+      ++cpu_count;
+    }
+  }
+
+  return cpu_count;
+}