From 518330069df68878934e0407a1c9e01036681d68 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Mon, 14 Mar 2022 15:41:42 +0100
Subject: bsps: Add gicv3_trigger_sgi()

---
 bsps/include/dev/irq/arm-gicv3.h | 21 +++++++++++++++++++++
 bsps/shared/dev/irq/arm-gicv3.c  | 15 +--------------
 2 files changed, 22 insertions(+), 14 deletions(-)

(limited to 'bsps')

diff --git a/bsps/include/dev/irq/arm-gicv3.h b/bsps/include/dev/irq/arm-gicv3.h
index 0583fded0a..cfc8cd3499 100644
--- a/bsps/include/dev/irq/arm-gicv3.h
+++ b/bsps/include/dev/irq/arm-gicv3.h
@@ -166,6 +166,27 @@ static inline bool gicv3_sgi_ppi_is_pending(
   return (sgi_ppi->icspispendr[0] & (1U << vector)) != 0;
 }
 
+static inline void gicv3_trigger_sgi(
+  rtems_vector_number vector,
+  uint32_t            targets
+)
+{
+#ifndef ARM_MULTILIB_ARCH_V4
+  uint64_t mpidr;
+#else
+  uint32_t mpidr;
+#endif
+  mpidr = READ_SR(MPIDR);
+  uint64_t value = ICC_SGIR_AFFINITY2(MPIDR_AFFINITY2_GET(mpidr))
+                 | ICC_SGIR_INTID(vector)
+                 | ICC_SGIR_AFFINITY1(MPIDR_AFFINITY1_GET(mpidr))
+                 | ICC_SGIR_CPU_TARGET_LIST(targets);
+#ifndef ARM_MULTILIB_ARCH_V4
+  value |= ICC_SGIR_AFFINITY3(MPIDR_AFFINITY3_GET(mpidr));
+#endif
+  WRITE64_SR(ICC_SGI1, value);
+}
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/bsps/shared/dev/irq/arm-gicv3.c b/bsps/shared/dev/irq/arm-gicv3.c
index 682af67b21..2f08d9bcb7 100644
--- a/bsps/shared/dev/irq/arm-gicv3.c
+++ b/bsps/shared/dev/irq/arm-gicv3.c
@@ -385,20 +385,7 @@ rtems_status_code bsp_interrupt_get_affinity(
 
 void arm_gic_trigger_sgi(rtems_vector_number vector, uint32_t targets)
 {
-#ifndef ARM_MULTILIB_ARCH_V4
-  uint64_t mpidr;
-#else
-  uint32_t mpidr;
-#endif
-  mpidr = READ_SR(MPIDR);
-  uint64_t value = ICC_SGIR_AFFINITY2(MPIDR_AFFINITY2_GET(mpidr))
-                 | ICC_SGIR_INTID(vector)
-                 | ICC_SGIR_AFFINITY1(MPIDR_AFFINITY1_GET(mpidr))
-                 | ICC_SGIR_CPU_TARGET_LIST(targets);
-#ifndef ARM_MULTILIB_ARCH_V4
-  value |= ICC_SGIR_AFFINITY3(MPIDR_AFFINITY3_GET(mpidr));
-#endif
-  WRITE64_SR(ICC_SGI1, value);
+  gicv3_trigger_sgi(vector, targets);
 }
 
 uint32_t arm_gic_irq_processor_count(void)
-- 
cgit v1.2.3