baremetal: aarch64 multicore works!!!

baremetal/arch/aarch64/multicore.S

@@ -9,17 +9,60 @@ main:
 
     /* Read cpu id into x1. */
     mrs x1, mpidr_el1
-    and x1, x1, #3
-    cbz x1, 1f
+    and x1, x1, 3
+    cbz x1, cpu0_only
+cpu1_only:
     /* Only CPU 1 reaches this point and sets the spinlock. */
-    mov x0, #1
+    mov x0, 1
     ldr x1, =spinlock
     str x0, [x1]
-    b .
-1:
+    /* Ensure that CPU 0 sees the write right now.
+     * Optional, but could save some useless CPU 1 loops.
+     */
+    dmb sy
+    /* Wake up CPU 0 if it is sleeping on wfe.
+     * Optional, but could save power on a real system.
+     */
+    sev
+cpu1_sleep_forever:
+    /* Hint CPU 1 to enter low power mode.
+     * Optional, but could save power on a real system.
+     */
+    wfe
+    b cpu1_sleep_forever
+cpu0_only:
     /* Only CPU 0 reaches this point. */
+
+#if !defined(GEM5)
+    /* Wake up CPU 1 from initial sleep!
+     * In gem5, CPU 1 starts woken up from the start,
+     * so this is not needed.
+     */
+    /* Function identifier: PCSI CPU_ON. */
+    ldr w0, =0xc4000003
+    /* Argument 1: target_cpu */
+    mov x1, 1
+    /* Argument 2: entry_point_address */
+    ldr x2, =cpu1_only
+    /* Argument 3: context_id */
+    mov x3, 0
+    /* Unused hvc args: the Linux kernel zeroes them,
+     * but I don't think it is required.
+     */
+#if 0
+    mov x4, 0
+    mov x5, 0
+    mov x6, 0
+    mov x7, 0
+#endif
+    hvc 0
+#endif
+
+spinlock_start:
     ldr x0, spinlock
-    cbz x0, 1b
+    /* Hint CPU 0 to enter low power mode. */
+    wfe
+    cbz x0, spinlock_start
 
     ret