From 4f217b19a9c67730c5400b4d513dfa469fec0ac7 Mon Sep 17 00:00:00 2001
From: Tomasz Gajewski <tomga@wp.pl>
Date: Sun, 12 Jan 2020 21:34:40 +0100
Subject: [PATCH] hw: add CPU wake up code for rpi3

Moved code waking up processors for Cortex A53 before changing privilege
level because sending events to higher privilege levels is not allowed.

Fixed enable_mmu for Cortex A53 to properly return cpu id.

Fixed starting code for secondary cores to properly initialize stacks.

Added code to wake up secondary cores on rpi3.

Ref #3573
---
 .../bootstrap/spec/arm_64/cortex_a53_mmu.cc   | 10 +++----
 .../base-hw/src/bootstrap/spec/arm_64/crt0.s  | 30 ++++++++++++-------
 .../src/bootstrap/spec/rpi3/platform.cc       | 12 ++++++--
 3 files changed, 35 insertions(+), 17 deletions(-)

diff --git a/repos/base-hw/src/bootstrap/spec/arm_64/cortex_a53_mmu.cc b/repos/base-hw/src/bootstrap/spec/arm_64/cortex_a53_mmu.cc
index 091e60dd0..e39618f63 100644
--- a/repos/base-hw/src/bootstrap/spec/arm_64/cortex_a53_mmu.cc
+++ b/repos/base-hw/src/bootstrap/spec/arm_64/cortex_a53_mmu.cc
@@ -16,7 +16,7 @@
 
 using Board::Cpu;
 
-extern "C" void * _crt0_enable_fpu;
+extern "C" void * _crt0_start_secondary;
 
 static inline void prepare_non_secure_world()
 {
@@ -144,6 +144,9 @@ unsigned Bootstrap::Platform::enable_mmu()
 	Cpu::Ttbr::access_t ttbr =
 		Cpu::Ttbr::Baddr::masked((Genode::addr_t)core_pd->table_base);
 
+	/* primary cpu wakes up all others */
+	if (primary && NR_OF_CPUS > 1) Cpu::wake_up_all_cpus(&_crt0_start_secondary);
+
 	while (Cpu::current_privilege_level() > Cpu::Current_el::EL1) {
 		if (Cpu::current_privilege_level() == Cpu::Current_el::EL3) {
 			prepare_non_secure_world();
@@ -153,9 +156,6 @@ unsigned Bootstrap::Platform::enable_mmu()
 		}
 	}
 
-	/* primary cpu wakes up all others */
-	if (primary && NR_OF_CPUS > 1) Cpu::wake_up_all_cpus(&_crt0_enable_fpu);
-
 	/* enable performance counter for user-land */
 	Cpu::Pmuserenr_el0::write(0b1111);
 	Cpu::Pmcr_el0::access_t pmcr = Cpu::Pmcr_el0::read();
@@ -201,5 +201,5 @@ unsigned Bootstrap::Platform::enable_mmu()
 	Cpu::Sctlr::Uct::set(sctlr, 1);
 	Cpu::Sctlr_el1::write(sctlr);
 
-	return 0;
+	return (Cpu::Mpidr::read() & 0xff);
 }
diff --git a/repos/base-hw/src/bootstrap/spec/arm_64/crt0.s b/repos/base-hw/src/bootstrap/spec/arm_64/crt0.s
index ce046ce0a..2bd36bf04 100644
--- a/repos/base-hw/src/bootstrap/spec/arm_64/crt0.s
+++ b/repos/base-hw/src/bootstrap/spec/arm_64/crt0.s
@@ -11,22 +11,25 @@
  * under the terms of the GNU Affero General Public License version 3.
  */
 
+/**
+ * Store CPU number in register x0
+ */
+.macro _cpu_number
+	mrs x0, mpidr_el1
+	and x0, x0, #0b11111111
+.endm
+
 .section ".text.crt0"
 
 	.global _start
 	_start:
 
-	/***********************
-	 ** Detect CPU number **
-	 ***********************/
-
-	mrs x0, mpidr_el1
-	and x0, x0, #0b11111111
-	cbz x0, _crt0_fill_bss_zero
-
 	/**
 	 * Hack for Qemu, which starts all cpus at once
+	 * only first CPU runs through, all others wait for wakeup
 	 */
+	_cpu_number
+	cbz x0, _crt0_fill_bss_zero
 	1:
 	ldr  x1, =_crt0_qemu_start_secondary_cpus
 	ldr  w1, [x1]
@@ -52,11 +55,18 @@
 	b 1b
 
 
+	/************************************
+	 ** Common Entrypoint for all CPUs **
+	 ************************************/
+
+	.global _crt0_start_secondary
+	_crt0_start_secondary:
+
+
 	/****************
 	 ** Enable FPU **
 	 ****************/
 
-	.global _crt0_enable_fpu
 	_crt0_enable_fpu:
 	mov x1, #0b11
 	lsl x1, x1, #20
@@ -69,6 +79,7 @@
 
 	.set STACK_SIZE, 0x2000
 
+	_cpu_number
 	ldr x1, =_crt0_start_stack
 	ldr x2, [x1]
 	mul x0, x0, x2
@@ -82,4 +93,3 @@
 	.endr
 	_crt0_start_stack:
 	.long STACK_SIZE
-
diff --git a/repos/base-hw/src/bootstrap/spec/rpi3/platform.cc b/repos/base-hw/src/bootstrap/spec/rpi3/platform.cc
index de7d6819f..bb6cfd3cf 100644
--- a/repos/base-hw/src/bootstrap/spec/rpi3/platform.cc
+++ b/repos/base-hw/src/bootstrap/spec/rpi3/platform.cc
@@ -30,8 +30,16 @@ Bootstrap::Platform::Board::Board()
 
 extern unsigned int _crt0_qemu_start_secondary_cpus;
 
-void Board::Cpu::wake_up_all_cpus(void *)
+void Board::Cpu::wake_up_all_cpus(void * ip)
 {
+	/* start when in qemu */
 	_crt0_qemu_start_secondary_cpus = 1;
-	asm volatile("dsb #0; sev");
+
+	/* start on real hardware */
+	*((void * volatile *) 0xe0) = ip;   /* cpu 1 */
+	*((void * volatile *) 0xe8) = ip;   /* cpu 2 */
+	*((void * volatile *) 0xf0) = ip;   /* cpu 3 */
+
+	/* send event for both variants */
+	asm volatile("dsb #15; sev");
 }