From 29919242dbc260fff2e82df9477003868aac7a71 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 22 Sep 2017 13:48:40 +0200
Subject: bsp/imx: Add SMP support

Update #3090.
---
 c/src/lib/libbsp/arm/imx/Makefile.am             |  1 +
 c/src/lib/libbsp/arm/imx/startup/bspsmp.c        | 38 ++++++++++++++----------
 c/src/lib/libbsp/arm/imx/startup/bspstarthooks.c |  9 ++++++
 3 files changed, 32 insertions(+), 16 deletions(-)

diff --git a/c/src/lib/libbsp/arm/imx/Makefile.am b/c/src/lib/libbsp/arm/imx/Makefile.am
index 20737ba7d4..dea1ef8013 100644
--- a/c/src/lib/libbsp/arm/imx/Makefile.am
+++ b/c/src/lib/libbsp/arm/imx/Makefile.am
@@ -88,6 +88,7 @@ libbsp_a_SOURCES += startup/bspreset.c
 libbsp_a_SOURCES += startup/bspstart.c
 libbsp_a_SOURCES += startup/ccm.c
 if HAS_SMP
+libbsp_a_SOURCES += ../shared/arm-a9mpcore-smp.c
 libbsp_a_SOURCES += startup/bspsmp.c
 endif
 
diff --git a/c/src/lib/libbsp/arm/imx/startup/bspsmp.c b/c/src/lib/libbsp/arm/imx/startup/bspsmp.c
index ac9561db05..51c7aef12a 100644
--- a/c/src/lib/libbsp/arm/imx/startup/bspsmp.c
+++ b/c/src/lib/libbsp/arm/imx/startup/bspsmp.c
@@ -14,28 +14,34 @@
 
 #include <rtems/score/smpimpl.h>
 
-uint32_t _CPU_SMP_Initialize(void)
-{
-  return 1;
-}
+#include <arm/freescale/imx/imx_srcreg.h>
+#include <arm/freescale/imx/imx_gpcreg.h>
+
+#include <bsp/start.h>
 
 bool _CPU_SMP_Start_processor(uint32_t cpu_index)
 {
-  (void) cpu_index;
+  bool started;
 
-  /* Nothing to do */
+  if (cpu_index == 1) {
+    volatile imx_src *src = (volatile imx_src *) 0x30390000;
+    volatile imx_gpc *gpc = (volatile imx_gpc *) 0x303a0000;
 
-  return true;
-}
+    src->gpr3 = (uint32_t) _start;
+    gpc->pgc_a7core0_ctrl |= IMX_GPC_PGC_CTRL_PCR;
+    gpc->cpu_pgc_sw_pup_req |= IMX_GPC_CPU_PGC_CORE1_A7;
 
-void _CPU_SMP_Finalize_initialization(uint32_t cpu_count)
-{
-}
+    while ((gpc->cpu_pgc_pup_status1 & IMX_GPC_CPU_PGC_CORE1_A7) != 0) {
+      /* Wait */
+    }
 
-void _CPU_SMP_Prepare_start_multitasking(void)
-{
-}
+    gpc->pgc_a7core0_ctrl &= ~IMX_GPC_PGC_CTRL_PCR;
+    src->a7rcr1 |= IMX_SRC_A7RCR1_A7_CORE1_ENABLE;
 
-void _CPU_SMP_Send_interrupt(uint32_t target_processor_index)
-{
+    started = true;
+  } else {
+    started = false;
+  }
+
+  return started;
 }
diff --git a/c/src/lib/libbsp/arm/imx/startup/bspstarthooks.c b/c/src/lib/libbsp/arm/imx/startup/bspstarthooks.c
index 2bbbae06df..c75b03415b 100644
--- a/c/src/lib/libbsp/arm/imx/startup/bspstarthooks.c
+++ b/c/src/lib/libbsp/arm/imx/startup/bspstarthooks.c
@@ -47,6 +47,15 @@ BSP_START_TEXT_SECTION static void setup_mmu_and_cache(void)
 
 BSP_START_TEXT_SECTION void bsp_start_hook_0(void)
 {
+#ifdef RTEMS_SMP
+  uint32_t cpu_id = arm_cortex_a9_get_multiprocessor_cpu_id();
+
+  arm_a9mpcore_start_enable_smp_in_auxiliary_control();
+
+  if (cpu_id != 0) {
+    arm_a9mpcore_start_on_secondary_processor();
+  }
+#endif
 }
 
 BSP_START_TEXT_SECTION void bsp_start_hook_1(void)
-- 
cgit v1.2.3