From c76c199f8dd821a43f638ee90dec12decbebc5ea Mon Sep 17 00:00:00 2001
From: Stefan Kalkowski <stefan.kalkowski@genode-labs.com>
Date: Mon, 23 Jan 2017 10:23:55 +0100
Subject: [PATCH] hw: enable SMP for Wandboard (fix #2253)

---
 repos/base-hw/lib/mk/spec/imx6/core-hw.mk     |  2 +-
 .../src/core/include/spec/imx6/board.h        | 61 +++++++++++++++----
 .../src/core/spec/cortex_a9/kernel/cpu.cc     |  6 ++
 .../include/spec/imx6/drivers/board_base.h    |  3 +
 4 files changed, 58 insertions(+), 14 deletions(-)

diff --git a/repos/base-hw/lib/mk/spec/imx6/core-hw.mk b/repos/base-hw/lib/mk/spec/imx6/core-hw.mk
index fd4386cc9c..40dc38f9b5 100644
--- a/repos/base-hw/lib/mk/spec/imx6/core-hw.mk
+++ b/repos/base-hw/lib/mk/spec/imx6/core-hw.mk
@@ -14,7 +14,7 @@ INC_DIR += $(REP_DIR)/src/core/include/spec/imx
 SRC_CC += platform_services.cc
 SRC_CC += spec/imx6/platform_support.cc
 
-NR_OF_CPUS = 1
+NR_OF_CPUS = 4
 
 # include less specific configuration
 include $(REP_DIR)/lib/mk/spec/cortex_a9/core-hw.inc
diff --git a/repos/base-hw/src/core/include/spec/imx6/board.h b/repos/base-hw/src/core/include/spec/imx6/board.h
index 540ee95011..14fb47b120 100644
--- a/repos/base-hw/src/core/include/spec/imx6/board.h
+++ b/repos/base-hw/src/core/include/spec/imx6/board.h
@@ -19,21 +19,56 @@
 #include <spec/imx/board_support.h>
 #include <spec/cortex_a9/board_support.h>
 
-namespace Genode
-{
-	/**
-	 * Board driver
-	 */
-	class Board : public Imx::Board, public Cortex_a9::Board
-	{
-		public:
+namespace Genode { class Board; }
 
-			void init()
+
+class Genode::Board : public Imx::Board, public Cortex_a9::Board
+{
+	private:
+
+		struct Src : Mmio
+		{
+			Src() : Mmio(SRC_MMIO_BASE) {}
+
+			struct Scr  : Register<0x0,  32>
 			{
-				Imx::Board::init();
-				Cortex_a9::Board::init();
+				struct Core_1_reset  : Bitfield<14,1> {};
+				struct Core_2_reset  : Bitfield<15,1> {};
+				struct Core_3_reset  : Bitfield<16,1> {};
+				struct Core_1_enable : Bitfield<22,1> {};
+				struct Core_2_enable : Bitfield<23,1> {};
+				struct Core_3_enable : Bitfield<24,1> {};
+			};
+			struct Gpr1 : Register<0x20, 32> {}; /* ep core 0 */
+			struct Gpr3 : Register<0x28, 32> {}; /* ep core 1 */
+			struct Gpr5 : Register<0x30, 32> {}; /* ep core 2 */
+			struct Gpr7 : Register<0x38, 32> {}; /* ep core 3 */
+
+			void entrypoint(void * entry)
+			{
+				write<Gpr3>((Gpr3::access_t)entry);
+				write<Gpr5>((Gpr5::access_t)entry);
+				write<Gpr7>((Gpr7::access_t)entry);
+				Scr::access_t v = read<Scr>();
+				Scr::Core_1_enable::set(v,1);
+				Scr::Core_1_reset::set(v,1);
+				Scr::Core_2_enable::set(v,1);
+				Scr::Core_3_reset::set(v,1);
+				Scr::Core_3_enable::set(v,1);
+				Scr::Core_3_reset::set(v,1);
+				write<Scr>(v);
 			}
-	};
-}
+		} _src;
+
+	public:
+
+		void init()
+		{
+			Imx::Board::init();
+			Cortex_a9::Board::init();
+		}
+
+		void wake_up_all_cpus(void *entry) { _src.entrypoint(entry); }
+};
 
 #endif /* _CORE__INCLUDE__SPEC__IMX6__BOARD_H_ */
diff --git a/repos/base-hw/src/core/spec/cortex_a9/kernel/cpu.cc b/repos/base-hw/src/core/spec/cortex_a9/kernel/cpu.cc
index a39fe448e4..74a08bd1a0 100644
--- a/repos/base-hw/src/core/spec/cortex_a9/kernel/cpu.cc
+++ b/repos/base-hw/src/core/spec/cortex_a9/kernel/cpu.cc
@@ -111,6 +111,12 @@ void Kernel::Cpu::init(Kernel::Pic &pic, Kernel::Pd & core_pd, Genode::Board & b
 	Actlr::enable_smp();
 	smp_coherency_enabled.inc();
 
+	/*
+	 * strangely, some older versions (imx6) seem to not work cache coherent
+	 * until SMP bit is set, so write back the variable here.
+	 */
+	clean_invalidate_inner_data_cache();
+
 	/* wait for other cores' coherency activation */
 	smp_coherency_enabled.wait_for(NR_OF_CPUS);
 
diff --git a/repos/base/include/spec/imx6/drivers/board_base.h b/repos/base/include/spec/imx6/drivers/board_base.h
index 3fa58265f5..28d9d4486e 100644
--- a/repos/base/include/spec/imx6/drivers/board_base.h
+++ b/repos/base/include/spec/imx6/drivers/board_base.h
@@ -66,6 +66,9 @@ struct Genode::Board_base
 		PL310_MMIO_BASE = 0x00a02000,
 		PL310_MMIO_SIZE = 0x00001000,
 
+		/* System reset controller */
+		SRC_MMIO_BASE = 0x20d8000,
+
 		/* CPU cache */
 		CACHE_LINE_SIZE_LOG2 = 5,