bsp/altera-cyclone-v: Move SMP support

3 files changed, 94 insertions, 54 deletions

diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am b/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
index 01c49b2859..78abb0b358 100644
--- a/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
@@ -149,7 +149,11 @@ libbsp_a_SOURCES += ../shared/arm-cp15-set-ttb-entries.c
 # Startup
 libbsp_a_SOURCES += startup/bspreset.c
 libbsp_a_SOURCES += startup/bspstart.c
+libbsp_a_SOURCES += startup/bspstarthooks.c
 libbsp_a_SOURCES += startup/nocache-heap.c
+if HAS_SMP
+libbsp_a_SOURCES += startup/bspsmp.c
+endif
 
 # IRQ
 libbsp_a_SOURCES += ../../shared/src/irq-default-handler.c
@@ -181,13 +185,6 @@ libbsp_a_SOURCES += ../shared/include/arm-cache-l1.h
 libbsp_a_SOURCES += ../shared/arm-l2c-310/cache_.h
 libbsp_a_CPPFLAGS += -I$(srcdir)/../shared/arm-l2c-310
 
-# Start hooks
-libbsp_a_SOURCES += startup/bspstarthooks.c
-
-if HAS_SMP
-libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
-endif
-
 ###############################################################################
 #                  Special Rules                                              #
 ###############################################################################
diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspsmp.c b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspsmp.c
new file mode 100644
index 0000000000..3b894b460f
--- /dev/null
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspsmp.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2013-2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <assert.h>
+
+#include <rtems/score/smpimpl.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp/irq.h>
+#include <bsp/linker-symbols.h>
+#include <bsp/start.h>
+
+#include <socal/alt_rstmgr.h>
+#include <socal/alt_sysmgr.h>
+#include <socal/hps.h>
+#include <socal/socal.h>
+
+static void bsp_inter_processor_interrupt(void *arg)
+{
+  _SMP_Inter_processor_interrupt_handler();
+}
+
+uint32_t _CPU_SMP_Initialize(void)
+{
+  uint32_t hardware_count = arm_gic_irq_processor_count();
+  uint32_t linker_count = (uint32_t) bsp_processor_count;
+
+  return hardware_count <= linker_count ? hardware_count : linker_count;
+}
+
+bool _CPU_SMP_Start_processor(uint32_t cpu_index)
+{
+  bool started;
+
+  if (cpu_index == 1) {
+    alt_write_word(
+      ALT_SYSMGR_ROMCODE_ADDR + ALT_SYSMGR_ROMCODE_CPU1STARTADDR_OFST,
+      ALT_SYSMGR_ROMCODE_CPU1STARTADDR_VALUE_SET((uint32_t) _start)
+    );
+
+    alt_clrbits_word(
+      ALT_RSTMGR_MPUMODRST_ADDR,
+      ALT_RSTMGR_MPUMODRST_CPU1_SET_MSK
+    );
+
+    started = true;
+  } else {
+    started = false;
+  }
+
+  return started;
+}
+
+void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
+{
+  if (cpu_count > 0) {
+    rtems_status_code sc;
+
+    sc = rtems_interrupt_handler_install(
+      ARM_GIC_IRQ_SGI_0,
+      "IPI",
+      RTEMS_INTERRUPT_UNIQUE,
+      bsp_inter_processor_interrupt,
+      NULL
+    );
+    assert(sc == RTEMS_SUCCESSFUL);
+  }
+}
+
+void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
+{
+  arm_gic_irq_generate_software_irq(
+    ARM_GIC_IRQ_SGI_0,
+    ARM_GIC_IRQ_SOFTWARE_IRQ_TO_ALL_IN_LIST,
+    (uint8_t) (1U << target_processor_index)
+  );
+}
diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspstart.c b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspstart.c
index 8722bb8838..a5e63e2d82 100644
--- a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspstart.c
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspstart.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013-2014 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -12,58 +12,12 @@
  * http://www.rtems.org/license/LICENSE.
  */
 
-#include <assert.h>
-#include <stdint.h>
-#include <bsp.h>
 #include <bsp/bootcard.h>
 #include <bsp/irq-generic.h>
-#include <bsp/linker-symbols.h>
-#include <bsp/start.h>
 #include <bsp/nocache-heap.h>
-#include <rtems/config.h>
-#include "socal/alt_rstmgr.h"
-#include "socal/alt_sysmgr.h"
-#include "socal/hps.h"
-
-#ifndef MIN
-#define MIN( a, b ) ( ( a ) < ( b ) ? ( a ) : ( b ) )
-#endif
-
-#define BSPSTART_MAX_CORES_PER_CONTROLLER 2
-
-static void bsp_start_secondary_cores( void )
-{
-#ifdef RTEMS_SMP
-  volatile uint32_t *mpumodrst       = ALT_RSTMGR_MPUMODRST_ADDR;
-  uint32_t          *cpu1_start_addr = (
-    ALT_SYSMGR_ROMCODE_ADDR + ALT_SYSMGR_ROMCODE_CPU1STARTADDR_OFST );
-  const uint32_t     CORES           = MIN(
-    (uintptr_t) bsp_processor_count,
-    rtems_configuration_get_maximum_processors() );
-  unsigned int       index;
-
-
-  /* Memory would get overwritten if a too small processor count
-   * would be specified */
-  assert( 
-    (uintptr_t) bsp_processor_count >= BSPSTART_MAX_CORES_PER_CONTROLLER );
-
-  if ( (uintptr_t) bsp_processor_count >= BSPSTART_MAX_CORES_PER_CONTROLLER ) {
-    for ( index = 1; index < CORES; ++index ) {
-      /* set the start address from where the core will execute */
-      (*cpu1_start_addr) = ALT_SYSMGR_ROMCODE_CPU1STARTADDR_VALUE_SET(
-        (uintptr_t) _start );
-
-      /* Make the core finish it's reset */
-      (*mpumodrst) &= ~ALT_RSTMGR_MPUMODRST_CPU1_SET_MSK;
-    }
-  }
-#endif /* #ifdef RTEMS_SMP */
-}
 
 void bsp_start( void )
 {
   bsp_interrupt_initialize();
   altera_cyclone_v_nocache_init_heap();
-  bsp_start_secondary_cores();
 }