1 files changed, 32 insertions, 2 deletions

diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index 8cfd5ab37743..9c2e1564e9a7 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -36,6 +36,7 @@
 #include <asm/pgtable-hwdef.h>
 #include <asm/pgtable.h>
 #include <asm/page.h>
+#include <asm/smp.h>
 #include <asm/sysreg.h>
 #include <asm/thread_info.h>
 #include <asm/virt.h>
@@ -643,7 +644,8 @@ __secondary_switched:
         msr     vbar_el1, x5
         isb
 
-        ldr_l   x0, secondary_data              // get secondary_data.stack
+        adr_l   x0, secondary_data
+        ldr     x0, [x0, #CPU_BOOT_STACK]       // get secondary_data.stack
         mov     sp, x0
         and     x0, x0, #~(THREAD_SIZE - 1)
         msr     sp_el0, x0                      // save thread_info
@@ -652,6 +654,29 @@ __secondary_switched:
 ENDPROC(__secondary_switched)
 
 /*
+ * The booting CPU updates the failed status @__early_cpu_boot_status,
+ * with MMU turned off.
+ *
+ * update_early_cpu_boot_status tmp, status
+ *  - Corrupts tmp1, tmp2
+ *  - Writes 'status' to __early_cpu_boot_status and makes sure
+ *    it is committed to memory.
+ */
+
+        .macro  update_early_cpu_boot_status status, tmp1, tmp2
+        mov     \tmp2, #\status
+        str_l   \tmp2, __early_cpu_boot_status, \tmp1
+        dmb     sy
+        dc      ivac, \tmp1                     // Invalidate potentially stale cache line
+        .endm
+
+        .pushsection    .data..cacheline_aligned
+        .align  L1_CACHE_SHIFT
+ENTRY(__early_cpu_boot_status)
+        .long   0
+        .popsection
+
+/*
  * Enable the MMU.
  *
  *  x0  = SCTLR_EL1 value for turning on the MMU.
@@ -669,6 +694,7 @@ ENTRY(__enable_mmu)
         ubfx    x2, x1, #ID_AA64MMFR0_TGRAN_SHIFT, 4
         cmp     x2, #ID_AA64MMFR0_TGRAN_SUPPORTED
         b.ne    __no_granule_support
+        update_early_cpu_boot_status 0, x1, x2
         msr     ttbr0_el1, x25                  // load TTBR0
         msr     ttbr1_el1, x26                  // load TTBR1
         isb
@@ -708,8 +734,12 @@ ENTRY(__enable_mmu)
 ENDPROC(__enable_mmu)
 
 __no_granule_support:
+        /* Indicate that this CPU can't boot and is stuck in the kernel */
+        update_early_cpu_boot_status CPU_STUCK_IN_KERNEL, x1, x2
+1:
         wfe
-        b __no_granule_support
+        wfi
+        b 1b
 ENDPROC(__no_granule_support)
 
 __primary_switch: