From 747fb65c6e5921c39c324c6e86ab2f2d87b47ee0 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 10 Dec 2020 09:09:10 +0100
Subject: bsps: Add GICv3 arm_gic_irq_processor_count()

Update #4202.
---
 bsps/include/dev/irq/arm-gic-irq.h |  7 +------
 bsps/shared/dev/irq/arm-gicv2.c    |  7 +++++++
 bsps/shared/dev/irq/arm-gicv3.c    | 27 +++++++++++++++++++++++++++
 3 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/bsps/include/dev/irq/arm-gic-irq.h b/bsps/include/dev/irq/arm-gic-irq.h
index 34bf34353e..d4b197bbb7 100644
--- a/bsps/include/dev/irq/arm-gic-irq.h
+++ b/bsps/include/dev/irq/arm-gic-irq.h
@@ -116,12 +116,7 @@ void arm_interrupt_handler_dispatch(rtems_vector_number vector);
  */
 void gicvx_interrupt_dispatch(void);
 
-static inline 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;
-}
+uint32_t arm_gic_irq_processor_count(void);
 
 void arm_gic_irq_initialize_secondary_cpu(void);
 
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;
+}
-- 
cgit v1.2.3