bsps/arm: Add SMP support

1 files changed, 27 insertions, 34 deletions

diff --git a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
index 84d66aa0fb..ed2d389589 100644
--- a/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
+++ b/c/src/lib/libbsp/arm/shared/arm-gic-irq.c
@@ -18,14 +18,12 @@
 
 #include <libcpu/arm-cp15.h>
 
-#include <bsp.h>
 #include <bsp/irq.h>
 #include <bsp/irq-generic.h>
+#include <bsp/start.h>
 
 #define GIC_CPUIF ((volatile gic_cpuif *) BSP_ARM_GIC_CPUIF_BASE)
 
-#define GIC_DIST ((volatile gic_dist *) BSP_ARM_GIC_DIST_BASE)
-
 #define PRIORITY_DEFAULT 128
 
 void bsp_interrupt_dispatch(void)
@@ -51,7 +49,7 @@ rtems_status_code bsp_interrupt_vector_enable(rtems_vector_number vector)
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_enable(dist, vector);
   } else {
@@ -66,7 +64,7 @@ rtems_status_code bsp_interrupt_vector_disable(rtems_vector_number vector)
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_disable(dist, vector);
   } else {
@@ -89,10 +87,15 @@ static inline uint32_t get_id_count(volatile gic_dist *dist)
 rtems_status_code bsp_interrupt_facility_initialize(void)
 {
   volatile gic_cpuif *cpuif = GIC_CPUIF;
-  volatile gic_dist *dist = GIC_DIST;
+  volatile gic_dist *dist = ARM_GIC_DIST;
   uint32_t id_count = get_id_count(dist);
   uint32_t id;
 
+  arm_cp15_set_exception_handler(
+    ARM_EXCEPTION_IRQ,
+    _ARMV4_Exception_interrupt
+  );
+
   for (id = 0; id < id_count; ++id) {
     gic_id_set_priority(dist, id, PRIORITY_DEFAULT);
   }
@@ -107,14 +110,25 @@ rtems_status_code bsp_interrupt_facility_initialize(void)
 
   dist->icddcr = GIC_DIST_ICDDCR_ENABLE;
 
-  arm_cp15_set_exception_handler(
-    ARM_EXCEPTION_IRQ,
-    _ARMV4_Exception_interrupt
-  );
-
   return RTEMS_SUCCESSFUL;
 }
 
+#ifdef RTEMS_SMP
+BSP_START_TEXT_SECTION void arm_gic_irq_initialize_secondary_cpu(void)
+{
+  volatile gic_cpuif *cpuif = GIC_CPUIF;
+  volatile gic_dist *dist = ARM_GIC_DIST;
+
+  while ((dist->icddcr & GIC_DIST_ICDDCR_ENABLE) == 0) {
+    /* Wait */
+  }
+
+  cpuif->iccpmr = GIC_CPUIF_ICCPMR_PRIORITY(0xff);
+  cpuif->iccbpr = GIC_CPUIF_ICCBPR_BINARY_POINT(0x0);
+  cpuif->iccicr = GIC_CPUIF_ICCICR_ENABLE;
+}
+#endif
+
 rtems_status_code arm_gic_irq_set_priority(
   rtems_vector_number vector,
   uint8_t priority
@@ -123,7 +137,7 @@ rtems_status_code arm_gic_irq_set_priority(
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     gic_id_set_priority(dist, vector, priority);
   } else {
@@ -141,7 +155,7 @@ rtems_status_code arm_gic_irq_get_priority(
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
   if (bsp_interrupt_is_valid_vector(vector)) {
-    volatile gic_dist *dist = GIC_DIST;
+    volatile gic_dist *dist = ARM_GIC_DIST;
 
     *priority = gic_id_get_priority(dist, vector);
   } else {
@@ -150,24 +164,3 @@ rtems_status_code arm_gic_irq_get_priority(
 
   return sc;
 }
-
-rtems_status_code arm_gic_irq_generate_software_irq(
-  rtems_vector_number vector,
-  arm_gic_irq_software_irq_target_filter filter,
-  uint8_t targets
-)
-{
-  rtems_status_code sc = RTEMS_SUCCESSFUL;
-
-  if (vector < 16) {
-    volatile gic_dist *dist = GIC_DIST;
-
-    dist->icdsgir = GIC_DIST_ICDSGIR_TARGET_LIST_FILTER(filter)
-      | GIC_DIST_ICDSGIR_CPU_TARGET_LIST(targets)
-      | GIC_DIST_ICDSGIR_SGIINTID(vector);
-  } else {
-    sc = RTEMS_INVALID_ID;
-  }
-
-  return sc;
-}