[Qemu-devel] [PATCH 1/3] hw/arm/boot: add secondary loader for AArch64

[Sergey Fedorov <serge.fdrv@gmail.com>]

This default secondary loader is used to bring up secondary CPUs using
spin table boot method.

Signed-off-by: Sergey Fedorov <serge.fdrv@gmail.com>
---
 hw/arm/boot.c | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/hw/arm/boot.c b/hw/arm/boot.c
index a48d1b2..2bff2f2 100644
--- a/hw/arm/boot.c
+++ b/hw/arm/boot.c
@@ -57,6 +57,18 @@ static const ARMInsnFixup bootloader_aarch64[] = {
     { 0, FIXUP_TERMINATOR }
 };
 
+static const ARMInsnFixup smpboot_aarch64[] = {
+    { 0xd503205f }, /* wfe */
+    { 0x580000a0 }, /* ldr x0, bootreg_addr */
+    { 0xf9400000 }, /* ldr x0, [x0] */
+    { 0xb4ffffa0 }, /* cbz x0, <wfe> */
+    { 0xd61f0000 }, /* br  x0 */
+    { 0xd503201f }, /* nop */
+    { 0, FIXUP_BOOTREG }, /* bootreg_addr: .word 0x... */
+    { 0 }, /* .word 0 */
+    { 0, FIXUP_TERMINATOR }
+};
+
 /* The worlds second smallest bootloader.  Set r0-r2, then jump to kernel.  */
 static const ARMInsnFixup bootloader[] = {
     { 0xe3a00000 }, /* mov     r0, #0 */
@@ -152,6 +164,7 @@ static void default_write_secondary(ARMCPU *cpu,
                                     const struct arm_boot_info *info)
 {
     uint32_t fixupcontext[FIXUP_MAX];
+    const ARMInsnFixup *secondary_loader;
 
     fixupcontext[FIXUP_GIC_CPU_IF] = info->gic_cpu_if_addr;
     fixupcontext[FIXUP_BOOTREG] = info->smp_bootreg_addr;
@@ -161,8 +174,13 @@ static void default_write_secondary(ARMCPU *cpu,
         fixupcontext[FIXUP_DSB] = CP15_DSB_INSN;
     }
 
+    if (arm_feature(&cpu->env, ARM_FEATURE_AARCH64)) {
+        secondary_loader = smpboot_aarch64;
+    } else {
+        secondary_loader = smpboot;
+    }
     write_bootloader("smpboot", info->smp_loader_start,
-                     smpboot, fixupcontext);
+                     secondary_loader, fixupcontext);
 }
 
 static void default_reset_secondary(ARMCPU *cpu,
@@ -171,7 +189,11 @@ static void default_reset_secondary(ARMCPU *cpu,
     CPUARMState *env = &cpu->env;
 
     stl_phys_notdirty(&address_space_memory, info->smp_bootreg_addr, 0);
-    env->regs[15] = info->smp_loader_start;
+    if (is_a64(env)) {
+        env->pc = info->smp_loader_start;
+    } else {
+        env->regs[15] = info->smp_loader_start;
+    }
 }
 
 static inline bool have_dtb(const struct arm_boot_info *info)
-- 
2.3.4