hw: implement multi-processor support for i.MX8

Fix #3520

repos/base-hw/lib/mk/spec/arm_v8/bootstrap-hw-imx8q_evk.mk

@@ -7,6 +7,8 @@ SRC_CC  += lib/base/arm_64/kernel/interface.cc
 SRC_CC  += spec/64bit/memory_map.cc
 SRC_S   += bootstrap/spec/arm_64/crt0.s
 
+NR_OF_CPUS = 4
+
 vpath spec/64bit/memory_map.cc $(BASE_DIR)/../base-hw/src/lib/hw
 
 include $(BASE_DIR)/../base-hw/lib/mk/bootstrap-hw.inc

repos/base-hw/lib/mk/spec/arm_v8/core-hw-imx8q_evk.mk

@@ -2,13 +2,13 @@ INC_DIR += $(REP_DIR)/src/core/spec/imx8q_evk
 INC_DIR += $(REP_DIR)/src/core/spec/arm_v8
 
 # add C++ sources
-SRC_CC += kernel/cpu_up.cc
+SRC_CC += kernel/cpu_mp.cc
-SRC_CC += kernel/lock.cc
 SRC_CC += kernel/vm_thread_off.cc
 SRC_CC += platform_services.cc
 SRC_CC += spec/64bit/memory_map.cc
 SRC_CC += spec/arm/generic_timer.cc
 SRC_CC += spec/arm/gicv3.cc
+SRC_CC += spec/arm/kernel/lock.cc
 SRC_CC += spec/arm/platform_support.cc
 SRC_CC += spec/arm_v8/cpu.cc
 SRC_CC += spec/arm_v8/kernel/cpu.cc
@@ -20,5 +20,7 @@ SRC_S += spec/arm_v8/crt0.s
 
 vpath spec/64bit/memory_map.cc $(BASE_DIR)/../base-hw/src/lib/hw
 
+NR_OF_CPUS = 4
+
 # include less specific configuration
 include $(REP_DIR)/lib/mk/core-hw.inc

repos/base-hw/src/bootstrap/spec/arm_64/crt0.s

@@ -23,8 +23,19 @@
 	mrs x0, mpidr_el1
 	and x0, x0, #0b11111111
 	cbz x0, _crt0_fill_bss_zero
+
+	/**
+	 * Hack for Qemu, which starts all cpus at once
+	 */
+	1:
+	ldr  x1, =_crt0_qemu_start_secondary_cpus
+	ldr  w1, [x1]
+	cbnz w1, _crt0_enable_fpu
 	wfe
-	b _start
+	b 1b
+	.global _crt0_qemu_start_secondary_cpus
+	_crt0_qemu_start_secondary_cpus:
+	.long 0
 
 
 	/***************************
@@ -32,12 +43,12 @@
 	 ***************************/
 
 	_crt0_fill_bss_zero:
-	ldr x0, =_bss_start
+	ldr x1, =_bss_start
-	ldr x1, =_bss_end
+	ldr x2, =_bss_end
 	1:
-	cmp x1, x0
+	cmp x2, x1
 	b.eq _crt0_enable_fpu
-	str xzr, [x0], #8
+	str xzr, [x1], #8
 	b 1b
 
 
@@ -45,20 +56,30 @@
 	 ** Enable FPU **
 	 ****************/
 
+	.global _crt0_enable_fpu
 	_crt0_enable_fpu:
-	mov x0, #0b11
+	mov x1, #0b11
-	lsl x0, x0, #20
+	lsl x1, x1, #20
-	msr cpacr_el1, x0
+	msr cpacr_el1, x1
 
 
 	/**********************
 	 ** Initialize stack **
 	 **********************/
 
-	ldr x0, =_crt0_start_stack
+	.set STACK_SIZE, 0x2000
-	mov sp, x0
+
+	ldr x1, =_crt0_start_stack
+	ldr x2, [x1]
+	mul x0, x0, x2
+	add x1, x1, x0
+	mov sp, x1
 	bl init
 
 	.p2align 4
-	.space 0x4000
+	.rept NR_OF_CPUS
+		.space STACK_SIZE
+	.endr
 	_crt0_start_stack:
+	.long STACK_SIZE
+

repos/base-hw/src/bootstrap/spec/imx8q_evk/board.h

@@ -21,7 +21,12 @@
 
 namespace Board {
 	using namespace Hw::Imx8q_evk_board;
-	using Cpu = Hw::Arm_64_cpu;
+
+	struct Cpu : Hw::Arm_64_cpu
+	{
+		static void wake_up_all_cpus(void*);
+	};
+
 	using Hw::Pic;
 };
 

repos/base-hw/src/bootstrap/spec/imx8q_evk/platform.cc

@@ -28,3 +28,22 @@ Bootstrap::Platform::Board::Board()
 {
 	::Board::Pic pic {};
 }
+
+
+void Board::Cpu::wake_up_all_cpus(void * ip)
+{
+	enum Function_id { CPU_ON = 0xC4000003 };
+
+	unsigned long result = 0;
+	for (unsigned i = 1; i < NR_OF_CPUS; i++) {
+		asm volatile("mov x0, %1  \n"
+		             "mov x1, %2  \n"
+		             "mov x2, %3  \n"
+		             "mov x3, %2  \n"
+		             "smc #0      \n"
+		             "mov %0, x0  \n"
+		             : "=r" (result) : "r" (CPU_ON), "r" (i), "r" (ip)
+		                      : "x0", "x1", "x2", "x3", "x4", "x5", "x6", "x7",
+		                        "x8", "x9", "x10", "x11", "x12", "x13", "x14");
+	}
+}

repos/base-hw/src/bootstrap/spec/rpi3/board.h

@@ -23,7 +23,7 @@ namespace Board {
 
 	struct Cpu : Hw::Arm_64_cpu
 	{
-		static void wake_up_all_cpus(void*);
+		static void wake_up_all_cpus(void*) {}
 	};
 
 	struct Pic { }; /* dummy object */

repos/base-hw/src/core/kernel/cpu_mp.cc

@@ -46,4 +46,6 @@ void Cpu::trigger_ip_interrupt()
 }
 
 
-Cpu::Ipi::Ipi(Cpu & cpu) : Irq(Board::Pic::IPI, cpu), cpu(cpu) { }
+Cpu::Ipi::Ipi(Cpu & cpu)
+: Irq(Board::Pic::IPI, cpu), cpu(cpu) {
+	cpu.pic().unmask(Board::Pic::IPI, cpu.id()); }

repos/base-hw/src/core/kernel/lock.h

@@ -31,7 +31,7 @@ class Kernel::Lock
 
 		enum State { UNLOCKED, LOCKED };
 
-		State volatile    _locked      { UNLOCKED };
+		int volatile      _locked      { UNLOCKED };
 		unsigned volatile _current_cpu { INVALID  };
 
 	public:

repos/base-hw/src/core/spec/arm_v8/cpu.h

@@ -83,7 +83,7 @@ struct Genode::Cpu : Hw::Arm_64_cpu
 	/**
 	 * Return kernel name of the executing CPU
 	 */
-	static unsigned executing_id() { return 0; }
+	static unsigned executing_id() { return Cpu::Mpidr::read() & 0xff; }
 
 
 	static void clean_data_cache_by_virt_region(addr_t, size_t);

repos/base-hw/src/core/spec/arm_v8/exception_vector.s

@@ -77,6 +77,7 @@ _kernel_entry:
 
 	.global idle_thread_main
 	idle_thread_main:
+	wfi
 	b idle_thread_main
 
 

repos/base-hw/src/include/hw/spec/arm/gicv3.h

@@ -161,6 +161,7 @@ class Hw::Pic
 			SYSTEM_REGISTER(32, Icc_pmr_el1,     "S3_0_C4_C6_0");
 			SYSTEM_REGISTER(32, Icc_igrpen1_el1, "S3_0_C12_C12_7");
 			SYSTEM_REGISTER(32, Icc_eoir1_el1,   "S3_0_C12_C12_1");
+			SYSTEM_REGISTER(64, Icc_sgi1r_el1,   "S3_0_C12_C11_5");
 
 			void init()
 			{
@@ -257,6 +258,11 @@ class Hw::Pic
 		}
 
 		void irq_mode(unsigned, unsigned, unsigned) { }
+
+		void send_ipi(unsigned const cpu_id)
+		{
+			Cpu_interface::Icc_sgi1r_el1::write(1ULL << cpu_id);
+		}
 };
 
 #undef SYSTEM_REGISTER

repos/base-hw/src/include/hw/spec/arm_64/cpu.h

@@ -97,6 +97,8 @@ struct Hw::Arm_64_cpu
 		struct Attr3 : Bitfield<24, 8> {};
 	);
 
+	SYSTEM_REGISTER(64, Mpidr, mpidr_el1);
+
 	SYSTEM_REGISTER(32, Pmuserenr_el0, pmuserenr_el0);
 
 	SYSTEM_REGISTER(64, Scr, scr_el3,
@@ -178,6 +180,37 @@ struct Hw::Arm_64_cpu
 	using Cntp_ctl  = Cntp_ctl_el0;
 	using Cntpct    = Cntpct_el0;
 	using Cntp_tval = Cntp_tval_el0;
+
+	static inline void wait_for_xchg(volatile int * addr,
+	                                 int new_value,
+	                                 int expected_value)
+	{
+		asm volatile(
+			/* check if load value of 'addr' is as expected */
+			"1: ldxr w7, [%0]     \n"
+			"cmp w7, %w2          \n"
+			"b.eq 2f              \n"
+
+			/* if not, wait for other CPU to send us an event */
+			"wfe                  \n"
+			"b.ne 1b              \n"
+
+			/* if yes, attempt to write 'new_value' to 'addr' */
+			"2: stxr w7, %w1, [%0]\n"
+
+			/* if write failed, restart */
+			"cbnz w7, 1b          \n"
+			"dmb #0               \n"
+		:: "r"(addr), "r"(new_value), "r"(expected_value) : "cc", "x7");
+	}
+
+	static inline void wakeup_waiting_cpus()
+	{
+		asm volatile(
+			"dsb #0 \n"
+			"sev    \n"
+		);
+	}
 };