bsps: Break out AArch32 GICv3 support

This breaks out AArch32-specific code so that the shared GICv3 code can
be reused by other architectures.

22 files changed, 177 insertions, 69 deletions

diff --git a/bsps/arm/altera-cyclone-v/include/bsp/irq.h b/bsps/arm/altera-cyclone-v/include/bsp/irq.h
index 4247d01747..bd2bba4caa 100644
--- a/bsps/arm/altera-cyclone-v/include/bsp/irq.h
+++ b/bsps/arm/altera-cyclone-v/include/bsp/irq.h
@@ -27,7 +27,7 @@
 #include <rtems/irq-extension.h>
 
 #include <bsp/arm-a9mpcore-irq.h>
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 #include <bsp/alt_interrupt_common.h>
 
 #ifdef __cplusplus
diff --git a/bsps/arm/altera-cyclone-v/include/tm27.h b/bsps/arm/altera-cyclone-v/include/tm27.h
index 23019a539a..00d7883f38 100644
--- a/bsps/arm/altera-cyclone-v/include/tm27.h
+++ b/bsps/arm/altera-cyclone-v/include/tm27.h
@@ -33,7 +33,7 @@
  * @brief Intel Cyclone V TM27 Support.
  */
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 /** @} */
 
diff --git a/bsps/arm/headers.am b/bsps/arm/headers.am
index f0d498c8f4..bff9a16fc8 100644
--- a/bsps/arm/headers.am
+++ b/bsps/arm/headers.am
@@ -17,10 +17,6 @@ include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-a9mpcore-regs.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-a9mpcore-start.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-cp15-start.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-errata.h
-include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-gic-irq.h
-include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-gic-regs.h
-include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-gic-tm27.h
-include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-gic.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-pl050-regs.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-pl050.h
 include_bsp_HEADERS += ../../../../../bsps/arm/include/bsp/arm-pl111-fb.h
diff --git a/bsps/arm/imx/include/bsp/irq.h b/bsps/arm/imx/include/bsp/irq.h
index 78b48e1613..1eea075bdc 100644
--- a/bsps/arm/imx/include/bsp/irq.h
+++ b/bsps/arm/imx/include/bsp/irq.h
@@ -20,7 +20,7 @@
 #include <rtems/irq.h>
 #include <rtems/irq-extension.h>
 
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/bsps/arm/imx/include/tm27.h b/bsps/arm/imx/include/tm27.h
index c17c0107b4..982ea594be 100644
--- a/bsps/arm/imx/include/tm27.h
+++ b/bsps/arm/imx/include/tm27.h
@@ -19,6 +19,6 @@
 #ifndef __tm27_h
 #define __tm27_h
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 #endif /* __tm27_h */
diff --git a/bsps/arm/include/bsp/arm-a9mpcore-start.h b/bsps/arm/include/bsp/arm-a9mpcore-start.h
index 8423e64e9d..a03bc8fb33 100644
--- a/bsps/arm/include/bsp/arm-a9mpcore-start.h
+++ b/bsps/arm/include/bsp/arm-a9mpcore-start.h
@@ -31,7 +31,7 @@
 #include <bsp/start.h>
 #include <bsp/arm-a9mpcore-regs.h>
 #include <bsp/arm-errata.h>
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/bsps/arm/realview-pbx-a9/include/bsp/irq.h b/bsps/arm/realview-pbx-a9/include/bsp/irq.h
index e66bf41df4..270ecd33ae 100644
--- a/bsps/arm/realview-pbx-a9/include/bsp/irq.h
+++ b/bsps/arm/realview-pbx-a9/include/bsp/irq.h
@@ -29,7 +29,7 @@
 #include <rtems/irq-extension.h>
 
 #include <bsp/arm-a9mpcore-irq.h>
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 /**
  * @defgroup realview-pbx-a9_interrupt Interrrupt Support
diff --git a/bsps/arm/realview-pbx-a9/include/tm27.h b/bsps/arm/realview-pbx-a9/include/tm27.h
index ae148aacdd..ecc0dc4c32 100644
--- a/bsps/arm/realview-pbx-a9/include/tm27.h
+++ b/bsps/arm/realview-pbx-a9/include/tm27.h
@@ -36,6 +36,6 @@
 #ifndef __tm27_h
 #define __tm27_h
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 #endif /* __tm27_h */
diff --git a/bsps/arm/shared/irq/irq-arm-gicv3-aarch32.c b/bsps/arm/shared/irq/irq-arm-gicv3-aarch32.c
new file mode 100644
index 0000000000..b805199ba9
--- /dev/null
+++ b/bsps/arm/shared/irq/irq-arm-gicv3-aarch32.c
@@ -0,0 +1,61 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ * @file
+ *
+ * @ingroup RTEMSBSPsARMShared
+ *
+ * @brief ARM-specific IRQ handlers.
+ */
+
+/*
+ * Copyright (C) 2020 On-Line Applications Research Corporation (OAR)
+ * Written by Kinsey Moore <kinsey.moore@oarcorp.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <libcpu/arm-cp15.h>
+#include <dev/irq/arm-gic-irq.h>
+#include <bsp/irq-generic.h>
+#include <rtems/score/armv4.h>
+
+void arm_interrupt_handler_dispatch(rtems_vector_number vector)
+{
+  uint32_t psr = _ARMV4_Status_irq_enable();
+  bsp_interrupt_handler_dispatch(vector);
+
+  _ARMV4_Status_restore(psr);
+}
+
+void arm_interrupt_facility_set_exception_handler(void)
+{
+  arm_cp15_set_exception_handler(
+    ARM_EXCEPTION_IRQ,
+    _ARMV4_Exception_interrupt
+  );
+}
+
+void bsp_interrupt_dispatch(void)
+{
+  gicv3_interrupt_dispatch();
+}
diff --git a/bsps/arm/shared/irq/irq-gic.c b/bsps/arm/shared/irq/irq-gic.c
index 42ae6c4d7d..1a401b67b6 100644
--- a/bsps/arm/shared/irq/irq-gic.c
+++ b/bsps/arm/shared/irq/irq-gic.c
@@ -12,7 +12,7 @@
  * http://www.rtems.org/license/LICENSE.
  */
 
-#include <bsp/arm-gic.h>
+#include <dev/irq/arm-gic.h>
 
 #include <rtems/score/armv4.h>
 
diff --git a/bsps/arm/xen/include/bsp/irq.h b/bsps/arm/xen/include/bsp/irq.h
index dc09e52373..a147a1c079 100644
--- a/bsps/arm/xen/include/bsp/irq.h
+++ b/bsps/arm/xen/include/bsp/irq.h
@@ -34,7 +34,7 @@
 #include <rtems/irq.h>
 #include <rtems/irq-extension.h>
 
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/bsps/arm/xen/include/tm27.h b/bsps/arm/xen/include/tm27.h
index 2fc4afa4f2..4c1dfaa213 100644
--- a/bsps/arm/xen/include/tm27.h
+++ b/bsps/arm/xen/include/tm27.h
@@ -33,6 +33,6 @@
 #ifndef __tm27_h
 #define __tm27_h
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 #endif /* __tm27_h */
diff --git a/bsps/arm/xilinx-zynq/include/bsp/irq.h b/bsps/arm/xilinx-zynq/include/bsp/irq.h
index edea29b7df..e707337fa1 100644
--- a/bsps/arm/xilinx-zynq/include/bsp/irq.h
+++ b/bsps/arm/xilinx-zynq/include/bsp/irq.h
@@ -40,7 +40,7 @@
 #include <rtems/irq-extension.h>
 
 #include <bsp/arm-a9mpcore-irq.h>
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/bsps/arm/xilinx-zynq/include/tm27.h b/bsps/arm/xilinx-zynq/include/tm27.h
index 885b97a30c..7fc8dd7edd 100644
--- a/bsps/arm/xilinx-zynq/include/tm27.h
+++ b/bsps/arm/xilinx-zynq/include/tm27.h
@@ -44,6 +44,6 @@
  * @brief Interrupt Mechanisms for tm27 test
  */
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 #endif /* __tm27_h */
diff --git a/bsps/arm/xilinx-zynqmp/include/bsp/irq.h b/bsps/arm/xilinx-zynqmp/include/bsp/irq.h
index 73567da011..6496d2312e 100644
--- a/bsps/arm/xilinx-zynqmp/include/bsp/irq.h
+++ b/bsps/arm/xilinx-zynqmp/include/bsp/irq.h
@@ -44,7 +44,7 @@
 #include <rtems/irq.h>
 #include <rtems/irq-extension.h>
 
-#include <bsp/arm-gic-irq.h>
+#include <dev/irq/arm-gic-irq.h>
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/bsps/arm/xilinx-zynqmp/include/tm27.h b/bsps/arm/xilinx-zynqmp/include/tm27.h
index 0f02a3b195..14214fe151 100644
--- a/bsps/arm/xilinx-zynqmp/include/tm27.h
+++ b/bsps/arm/xilinx-zynqmp/include/tm27.h
@@ -49,6 +49,6 @@
  * @brief Interrupt Mechanisms for tm27 test
  */
 
-#include <bsp/arm-gic-tm27.h>
+#include <dev/irq/arm-gic-tm27.h>
 
 #endif /* __tm27_h */
diff --git a/bsps/headers.am b/bsps/headers.am
index aaf13284bd..b433c2d8d4 100644
--- a/bsps/headers.am
+++ b/bsps/headers.am
@@ -21,6 +21,13 @@ include_bsp_HEADERS += ../../bsps/include/bsp/u-boot.h
 include_bsp_HEADERS += ../../bsps/include/bsp/uart-output-char.h
 include_bsp_HEADERS += ../../bsps/include/bsp/utility.h
 
+include_dev_irqdir = $(includedir)/dev/irq
+include_dev_irq_HEADERS =
+include_dev_irq_HEADERS += ../../bsps/include/dev/irq/arm-gic-irq.h
+include_dev_irq_HEADERS += ../../bsps/include/dev/irq/arm-gic-regs.h
+include_dev_irq_HEADERS += ../../bsps/include/dev/irq/arm-gic-tm27.h
+include_dev_irq_HEADERS += ../../bsps/include/dev/irq/arm-gic.h
+
 include_dev_serialdir = $(includedir)/dev/serial
 include_dev_serial_HEADERS =
 include_dev_serial_HEADERS += ../../bsps/include/dev/serial/arm-pl011-regs.h
diff --git a/bsps/arm/include/bsp/arm-gic-irq.h b/bsps/include/dev/irq/arm-gic-irq.h
index 219c3c7189..a97191faca 100644
--- a/bsps/arm/include/bsp/arm-gic-irq.h
+++ b/bsps/include/dev/irq/arm-gic-irq.h
@@ -24,7 +24,7 @@
 #define LIBBSP_ARM_SHARED_ARM_GIC_IRQ_H
 
 #include <bsp.h>
-#include <bsp/arm-gic.h>
+#include <dev/irq/arm-gic.h>
 #include <rtems/score/processormask.h>
 
 #ifdef __cplusplus
@@ -108,6 +108,25 @@ static inline rtems_status_code arm_gic_irq_generate_software_irq(
   return sc;
 }
 
+/**
+ * This architecture-specific function sets the exception vector for handling
+ * IRQs.
+ */
+void arm_interrupt_facility_set_exception_handler(void);
+
+/**
+ * This architecture-specific function dispatches a triggered IRQ.
+ *
+ * @param[in] vector The vector on which the IRQ occurred.
+ */
+void arm_interrupt_handler_dispatch(rtems_vector_number vector);
+
+/**
+ * This is the GICv3 interrupt dispatcher that is to be called from the
+ * architecture-specific implementation of the IRQ handler.
+ */
+void gicv3_interrupt_dispatch(void);
+
 static inline uint32_t arm_gic_irq_processor_count(void)
 {
   volatile gic_dist *dist = ARM_GIC_DIST;
diff --git a/bsps/arm/include/bsp/arm-gic-regs.h b/bsps/include/dev/irq/arm-gic-regs.h
index 8a65294b6f..8a65294b6f 100644
--- a/bsps/arm/include/bsp/arm-gic-regs.h
+++ b/bsps/include/dev/irq/arm-gic-regs.h
diff --git a/bsps/arm/include/bsp/arm-gic-tm27.h b/bsps/include/dev/irq/arm-gic-tm27.h
index 95f3077716..95f3077716 100644
--- a/bsps/arm/include/bsp/arm-gic-tm27.h
+++ b/bsps/include/dev/irq/arm-gic-tm27.h
diff --git a/bsps/arm/include/bsp/arm-gic.h b/bsps/include/dev/irq/arm-gic.h
index 42d53284c0..23c70e7b0e 100644
--- a/bsps/arm/include/bsp/arm-gic.h
+++ b/bsps/include/dev/irq/arm-gic.h
@@ -23,7 +23,7 @@
 #ifndef LIBBSP_ARM_SHARED_ARM_GIC_H
 #define LIBBSP_ARM_SHARED_ARM_GIC_H
 
-#include <bsp/arm-gic-regs.h>
+#include <dev/irq/arm-gic-regs.h>
 
 #include <stdbool.h>
 
diff --git a/bsps/arm/shared/irq/irq-gicv3.c b/bsps/shared/dev/irq/arm-gicv3.c
index 138b565b9b..ad39872eb0 100644
--- a/bsps/arm/shared/irq/irq-gicv3.c
+++ b/bsps/shared/dev/irq/arm-gicv3.c
@@ -25,11 +25,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <bsp/arm-gic.h>
-
-#include <rtems/score/armv4.h>
-
-#include <libcpu/arm-cp15.h>
+#include <dev/irq/arm-gic.h>
 
 #include <bsp/irq.h>
 #include <bsp/irq-generic.h>
@@ -37,6 +33,34 @@
 
 #define PRIORITY_DEFAULT 127
 
+#define MPIDR_AFFINITY2(val) BSP_FLD64(val, 16, 23)
+#define MPIDR_AFFINITY2_GET(reg) BSP_FLD64GET(reg, 16, 23)
+#define MPIDR_AFFINITY2_SET(reg, val) BSP_FLD64SET(reg, val, 16, 23)
+#define MPIDR_AFFINITY1(val) BSP_FLD64(val, 8, 15)
+#define MPIDR_AFFINITY1_GET(reg) BSP_FLD64GET(reg, 8, 15)
+#define MPIDR_AFFINITY1_SET(reg, val) BSP_FLD64SET(reg, val, 8, 15)
+#define MPIDR_AFFINITY0(val) BSP_FLD64(val, 0, 7)
+#define MPIDR_AFFINITY0_GET(reg) BSP_FLD64GET(reg, 0, 7)
+#define MPIDR_AFFINITY0_SET(reg, val) BSP_FLD64SET(reg, val, 0, 7)
+
+#define ICC_SGIR_AFFINITY3(val) BSP_FLD64(val, 48, 55)
+#define ICC_SGIR_AFFINITY3_GET(reg) BSP_FLD64GET(reg, 48, 55)
+#define ICC_SGIR_AFFINITY3_SET(reg, val) BSP_FLD64SET(reg, val, 48, 55)
+#define ICC_SGIR_IRM BSP_BIT32(40)
+#define ICC_SGIR_AFFINITY2(val) BSP_FLD64(val, 32, 39)
+#define ICC_SGIR_AFFINITY2_GET(reg) BSP_FLD64GET(reg, 32, 39)
+#define ICC_SGIR_AFFINITY2_SET(reg, val) BSP_FLD64SET(reg, val, 32, 39)
+#define ICC_SGIR_INTID(val) BSP_FLD64(val, 24, 27)
+#define ICC_SGIR_INTID_GET(reg) BSP_FLD64GET(reg, 24, 27)
+#define ICC_SGIR_INTID_SET(reg, val) BSP_FLD64SET(reg, val, 24, 27)
+#define ICC_SGIR_AFFINITY1(val) BSP_FLD64(val, 16, 23)
+#define ICC_SGIR_AFFINITY1_GET(reg) BSP_FLD64GET(reg, 16, 23)
+#define ICC_SGIR_AFFINITY1_SET(reg, val) BSP_FLD64SET(reg, val, 16, 23)
+#define ICC_SGIR_CPU_TARGET_LIST(val) BSP_FLD64(val, 0, 15)
+#define ICC_SGIR_CPU_TARGET_LIST_GET(reg) BSP_FLD64GET(reg, 0, 15)
+#define ICC_SGIR_CPU_TARGET_LIST_SET(reg, val) BSP_FLD64SET(reg, val, 0, 15)
+
+#ifdef ARM_MULTILIB_ARCH_V4
 /* cpuif->iccicr */
 #define ICC_CTLR    "p15, 0, %0, c12, c12, 4"
 
@@ -60,40 +84,8 @@
 #define ICC_IGRPEN0 "p15, 0, %0, c12, c12, 6"
 #define ICC_IGRPEN1 "p15, 0, %0, c12, c12, 7"
 
-#define ICC_SGI1    "p15, 0, %Q0, %R0, c12"
-
-#define ICC_SGIR_AFFINITY3(val) BSP_FLD64(val, 48, 55)
-#define ICC_SGIR_AFFINITY3_GET(reg) BSP_FLD64GET(reg, 48, 55)
-#define ICC_SGIR_AFFINITY3_SET(reg, val) BSP_FLD64SET(reg, val, 48, 55)
-#define ICC_SGIR_IRM BSP_BIT32(40)
-#define ICC_SGIR_AFFINITY2(val) BSP_FLD64(val, 32, 39)
-#define ICC_SGIR_AFFINITY2_GET(reg) BSP_FLD64GET(reg, 32, 39)
-#define ICC_SGIR_AFFINITY2_SET(reg, val) BSP_FLD64SET(reg, val, 32, 39)
-#define ICC_SGIR_INTID(val) BSP_FLD64(val, 24, 27)
-#define ICC_SGIR_INTID_GET(reg) BSP_FLD64GET(reg, 24, 27)
-#define ICC_SGIR_INTID_SET(reg, val) BSP_FLD64SET(reg, val, 24, 27)
-#define ICC_SGIR_AFFINITY1(val) BSP_FLD64(val, 16, 23)
-#define ICC_SGIR_AFFINITY1_GET(reg) BSP_FLD64GET(reg, 16, 23)
-#define ICC_SGIR_AFFINITY1_SET(reg, val) BSP_FLD64SET(reg, val, 16, 23)
-#define ICC_SGIR_CPU_TARGET_LIST(val) BSP_FLD64(val, 0, 15)
-#define ICC_SGIR_CPU_TARGET_LIST_GET(reg) BSP_FLD64GET(reg, 0, 15)
-#define ICC_SGIR_CPU_TARGET_LIST_SET(reg, val) BSP_FLD64SET(reg, val, 0, 15)
-
 #define MPIDR       "p15, 0, %0, c0, c0, 5"
 
-#define MPIDR_AFFINITY3(val) BSP_FLD64(val, 25, 29)
-#define MPIDR_AFFINITY3_GET(reg) BSP_FLD64GET(reg, 25, 29)
-#define MPIDR_AFFINITY3_SET(reg, val) BSP_FLD64SET(reg, val, 25, 29)
-#define MPIDR_AFFINITY2(val) BSP_FLD64(val, 16, 23)
-#define MPIDR_AFFINITY2_GET(reg) BSP_FLD64GET(reg, 16, 23)
-#define MPIDR_AFFINITY2_SET(reg, val) BSP_FLD64SET(reg, val, 16, 23)
-#define MPIDR_AFFINITY1(val) BSP_FLD64(val, 8, 15)
-#define MPIDR_AFFINITY1_GET(reg) BSP_FLD64GET(reg, 8, 15)
-#define MPIDR_AFFINITY1_SET(reg, val) BSP_FLD64SET(reg, val, 8, 15)
-#define MPIDR_AFFINITY0(val) BSP_FLD64(val, 0, 7)
-#define MPIDR_AFFINITY0_GET(reg) BSP_FLD64GET(reg, 0, 7)
-#define MPIDR_AFFINITY0_SET(reg, val) BSP_FLD64SET(reg, val, 0, 7)
-
 #define READ_SR(SR_NAME) \
 ({ \
   uint32_t value; \
@@ -104,23 +96,52 @@
 #define WRITE_SR(SR_NAME, VALUE) \
     __asm__ volatile("mcr    " SR_NAME "  \n" : : "r" (VALUE) );
 
+#define ICC_SGI1    "p15, 0, %Q0, %R0, c12"
 #define WRITE64_SR(SR_NAME, VALUE) \
     __asm__ volatile("mcrr    " SR_NAME "  \n" : : "r" (VALUE) );
 
+#else /* ARM_MULTILIB_ARCH_V4 */
+
+/* AArch64 GICv3 registers are not named in GCC */
+#define ICC_IGRPEN0 "S3_0_C12_C12_6, %0"
+#define ICC_IGRPEN1 "S3_0_C12_C12_7, %0"
+#define ICC_PMR     "S3_0_C4_C6_0, %0"
+#define ICC_EOIR1   "S3_0_C12_C12_1, %0"
+#define ICC_SRE     "S3_0_C12_C12_5, %0"
+#define ICC_BPR0    "S3_0_C12_C8_3, %0"
+#define ICC_CTLR    "S3_0_C12_C12_4, %0"
+#define ICC_IAR1    "%0, S3_0_C12_C12_0"
+#define MPIDR       "%0, mpidr_el1"
+#define MPIDR_AFFINITY3(val) BSP_FLD64(val, 32, 39)
+#define MPIDR_AFFINITY3_GET(reg) BSP_FLD64GET(reg, 32, 39)
+#define MPIDR_AFFINITY3_SET(reg, val) BSP_FLD64SET(reg, val, 32, 39)
+
+#define ICC_SGI1    "S3_0_C12_C11_5, %0"
+#define WRITE64_SR(SR_NAME, VALUE) \
+    __asm__ volatile("msr    " SR_NAME "  \n" : : "r" (VALUE) );
+#define WRITE_SR(SR_NAME, VALUE) WRITE64_SR(SR_NAME, VALUE)
+
+#define READ_SR(SR_NAME) \
+({ \
+  uint64_t value; \
+  __asm__ volatile("mrs    " SR_NAME : "=&r" (value) ); \
+  value; \
+})
+
+
+#endif /* ARM_MULTILIB_ARCH_V4 */
+
 #define ARM_GIC_REDIST ((volatile gic_redist *) BSP_ARM_GIC_REDIST_BASE)
 #define ARM_GIC_SGI_PPI (((volatile gic_sgi_ppi *) ((char*)BSP_ARM_GIC_REDIST_BASE + (1 << 16))))
 
-void bsp_interrupt_dispatch(void)
+void gicv3_interrupt_dispatch(void)
 {
   uint32_t icciar = READ_SR(ICC_IAR1);
   rtems_vector_number vector = GIC_CPUIF_ICCIAR_ACKINTID_GET(icciar);
   rtems_vector_number spurious = 1023;
 
   if (vector != spurious) {
-    uint32_t psr = _ARMV4_Status_irq_enable();
-    bsp_interrupt_handler_dispatch(vector);
-
-    _ARMV4_Status_restore(psr);
+    arm_interrupt_handler_dispatch(vector);
 
     WRITE_SR(ICC_EOIR1, icciar);
   }
@@ -199,10 +220,7 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
   uint32_t id_count = get_id_count(dist);
   uint32_t id;
 
-  arm_cp15_set_exception_handler(
-    ARM_EXCEPTION_IRQ,
-    _ARMV4_Exception_interrupt
-  );
+  arm_interrupt_facility_set_exception_handler();
 
   dist->icddcr = GIC_DIST_ICDDCR_ARE_NS | GIC_DIST_ICDDCR_ARE_S
                | GIC_DIST_ICDDCR_ENABLE_GRP1S | GIC_DIST_ICDDCR_ENABLE_GRP1NS
@@ -319,11 +337,18 @@ void arm_gic_trigger_sgi(
    * ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
    * ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_EXCEPT_SELF,
    * ARM_GIC_IRQ_SOFTWARE_IRQ_TO_SELF */
-  uint32_t mpidr = READ_SR(MPIDR);
-  uint64_t value = ICC_SGIR_AFFINITY3(MPIDR_AFFINITY3_GET(mpidr))
-                 | ICC_SGIR_AFFINITY2(MPIDR_AFFINITY2_GET(mpidr))
+#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(1);
+#ifndef ARM_MULTILIB_ARCH_V4
+  value |= ICC_SGIR_AFFINITY3(MPIDR_AFFINITY3_GET(mpidr));
+#endif
   WRITE64_SR(ICC_SGI1, value);
 }