@@ -63,6 +63,7 @@ static inline unsigned long read_cpsr(void)
#define SCR "p15, 0, %0, c1, c1, 0"
#define NSACR "p15, 0, %0, c1, c1, 2"
#define ICIALLU "p15, 0, %0, c7, c5, 0"
+#define MVBAR "p15, 0, %0, c12, c0, 1"
#define ICC_SRE "p15, 6, %0, c12, c12, 5"
#define ICC_CTLR "p15, 6, %0, c12, c12, 4"
new file mode 100644
@@ -0,0 +1,28 @@
+/*
+ * arch/aarch64/include/asm/psci.h
+ *
+ * Copyright (C) 2021 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#ifndef __ASM_AARCH64_PSCI_H
+#define __ASM_AARCH64_PSCI_H
+
+#include <cpu.h>
+#include <stdbool.h>
+
+extern char psci_vectors[];
+
+static inline bool cpu_init_psci_arch(void)
+{
+ if (read_cpsr_mode() != PSR_MON)
+ return false;
+
+ mcr(MVBAR, (unsigned long)psci_vectors);
+ isb();
+
+ return true;
+}
+
+#endif
@@ -15,7 +15,7 @@
.section .vectors
.align 6
-smc_vectors:
+ASM_DATA(psci_vectors)
b err_exception @ Reset
b err_exception @ Undef
b handle_smc @ SMC
@@ -39,11 +39,5 @@ handle_smc:
movs pc, lr
ASM_FUNC(start_el3)
- ldr r0, =smc_vectors
- blx setup_vector
- /* pass through */
-
ASM_FUNC(start_no_el3)
- cpuid r0, r1
- blx find_logical_id
b psci_first_spin
@@ -35,12 +35,3 @@ ASM_FUNC(find_logical_id)
bx lr
3: mov r0, #MPIDR_INVALID
bx lr
-
-/*
- * Setup EL3 vectors.
- * r0: vector address
- */
-ASM_FUNC(setup_vector)
- mcr p15, 0, r0, c12, c0, 1 @ MVBAR
- isb
- bx lr
new file mode 100644
@@ -0,0 +1,28 @@
+/*
+ * arch/aarch64/include/asm/psci.h
+ *
+ * Copyright (C) 2021 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#ifndef __ASM_AARCH64_PSCI_H
+#define __ASM_AARCH64_PSCI_H
+
+#include <cpu.h>
+#include <stdbool.h>
+
+extern char psci_vectors[];
+
+static inline bool cpu_init_psci_arch(void)
+{
+ if (mrs(CurrentEL) != CURRENTEL_EL3)
+ return false;
+
+ msr(VBAR_EL3, (unsigned long)psci_vectors);
+ isb();
+
+ return true;
+}
+
+#endif
@@ -19,7 +19,7 @@
.section .vectors, "w"
.align 11
-vector:
+ASM_DATA(psci_vectors)
// current EL, SP_EL0
ventry err_exception // synchronous
ventry err_exception // IRQ
@@ -80,22 +80,5 @@ smc_exit:
eret
ASM_FUNC(start_el3)
- ldr x0, =vector
- bl setup_vector
-
- /* only boot the primary cpu (entry 0 in the table) */
- cpuid x0, x1
- bl find_logical_id
- b psci_first_spin
-
-/*
- * This PSCI implementation requires EL3. Without EL3 we'll only boot the
- * primary cpu, all others will be trapped in an infinite loop.
- */
ASM_FUNC(start_no_el3)
- cpuid x0, x1
- bl find_logical_id
- cbz x0, psci_first_spin
-spin_dead:
- wfe
- b spin_dead
+ b psci_first_spin
@@ -12,6 +12,9 @@
.text
+ASM_FUNC(cpu_init_bootmethod)
+ ret
+
ASM_FUNC(start_el3)
ASM_FUNC(start_no_el3)
cpuid x0, x1
@@ -32,12 +32,3 @@ ASM_FUNC(find_logical_id)
ret
3: mov x0, #MPIDR_INVALID
ret
-
-/*
- * Setup EL3 vectors
- * x0: vector address
- */
-ASM_FUNC(setup_vector)
- msr VBAR_EL3, x0
- isb
- ret
@@ -6,6 +6,7 @@
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE.txt file.
*/
+#include <boot.h>
#include <cpu.h>
#include <platform.h>
@@ -44,7 +45,9 @@ void announce_arch(void);
void cpu_init_bootwrapper(void)
{
- if (this_cpu_logical_id() == 0) {
+ unsigned int cpu = this_cpu_logical_id();
+
+ if (cpu == 0) {
init_uart();
announce_bootwrapper();
announce_arch();
@@ -52,4 +55,6 @@ void cpu_init_bootwrapper(void)
print_string("\r\n");
init_platform();
}
+
+ cpu_init_bootmethod(cpu);
}
@@ -12,8 +12,11 @@
#include <bakery_lock.h>
#include <boot.h>
#include <cpu.h>
+#include <platform.h>
#include <psci.h>
+#include <asm/psci.h>
+
#ifndef CPU_IDS
#error "No MPIDRs provided"
#endif
@@ -78,12 +81,25 @@ long psci_call(unsigned long fid, unsigned long arg1, unsigned long arg2)
}
}
-void __noreturn psci_first_spin(unsigned int cpu)
+void __noreturn psci_first_spin(void)
{
- if (cpu == MPIDR_INVALID)
- while (1);
+ unsigned int cpu = this_cpu_logical_id();
first_spin(cpu, branch_table + cpu, PSCI_ADDR_INVALID);
unreachable();
}
+
+void cpu_init_bootmethod(unsigned int cpu)
+{
+ if (cpu_init_psci_arch())
+ return;
+
+ if (cpu == 0) {
+ print_string("WARNING: PSCI could not be initialized. Boot may fail\r\n\r\n");
+ return;
+ }
+
+ while (1)
+ wfe();
+}
@@ -16,4 +16,6 @@ void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry);
void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
unsigned long invalid_addr);
+void cpu_init_bootmethod(unsigned int cpu);
+
#endif
We currently initialize the bootmethod late, in assembly code. This requires us to maintain the el3/no_el3 distintion late into the boot process, and means we cannot produce any helpful diagnostic when booted at an unexpected exception level. Rework things so that we initialize the bootmethod early, with a warning when things are wrong. The el3/no_el3 distinction is now irrelevant to the bootmethod code, and can be removed in subsequent patches. When a boot-wrapper configured for PSCI is entered at EL2, a warning is looged to the serial console as: | Boot-wrapper v0.2 | Entered at EL2 | Memory layout: | [0000000080000000..0000000080001f90] => boot-wrapper | [000000008000fff8..0000000080010000] => mbox | [0000000080200000..00000000822af200] => kernel | [0000000088000000..0000000088002857] => dtb | | WARNING: PSCI could not be initialized. Boot may fail Signed-off-by: Mark Rutland <mark.rutland@arm.com> --- arch/aarch32/include/asm/cpu.h | 1 + arch/aarch32/include/asm/psci.h | 28 ++++++++++++++++++++++++++++ arch/aarch32/psci.S | 8 +------- arch/aarch32/utils.S | 9 --------- arch/aarch64/include/asm/psci.h | 28 ++++++++++++++++++++++++++++ arch/aarch64/psci.S | 21 ++------------------- arch/aarch64/spin.S | 3 +++ arch/aarch64/utils.S | 9 --------- common/init.c | 7 ++++++- common/psci.c | 22 +++++++++++++++++++--- include/boot.h | 2 ++ 11 files changed, 90 insertions(+), 48 deletions(-) create mode 100644 arch/aarch32/include/asm/psci.h create mode 100644 arch/aarch64/include/asm/psci.h