5 files changed, 126 insertions, 0 deletions

diff --git a/src/arch/arm64/cpu-stubs.c b/src/arch/arm64/cpu-stubs.c
index c7b083edfd..da767599ff 100644
--- a/src/arch/arm64/cpu-stubs.c
+++ b/src/arch/arm64/cpu-stubs.c
@@ -18,8 +18,14 @@
  */
 
 void arm64_cpu_early_setup(void);
+void cortex_a57_cpu_power_down(int l2_flush);
 
 void __attribute__((weak)) arm64_cpu_early_setup(void)
 {
 	/* Default empty implementation */
 }
+
+void __attribute__((weak)) cortex_a57_cpu_power_down(int l2_flush)
+{
+	/* Default empty implementation */
+}
diff --git a/src/arch/arm64/cpu/Kconfig b/src/arch/arm64/cpu/Kconfig
index fadd48a0d7..20304be4e5 100644
--- a/src/arch/arm64/cpu/Kconfig
+++ b/src/arch/arm64/cpu/Kconfig
@@ -21,3 +21,8 @@ config ARCH_ARM64_CPU_CORTEX_A57
 	bool
 	default n
 	depends on ARCH_ARM64
+
+config ARCH_ARM64_CORTEX_A57_POWER_DOWN_SUPPORT
+	bool
+	default n
+	depends on ARCH_ARM64 && ARCH_ARM64_CPU_CORTEX_A57
diff --git a/src/arch/arm64/cpu/cortex_a57.S b/src/arch/arm64/cpu/cortex_a57.S
index 5039d1c59e..ae0517569f 100644
--- a/src/arch/arm64/cpu/cortex_a57.S
+++ b/src/arch/arm64/cpu/cortex_a57.S
@@ -18,6 +18,7 @@
  */
 
 #include <arch/asm.h>
+#include <arch/cache_helpers.h>
 #include "cortex_a57.h"
 
 ENTRY(arm64_cpu_early_setup)
@@ -27,3 +28,97 @@ ENTRY(arm64_cpu_early_setup)
 	isb
 	ret
 ENDPROC(arm64_cpu_early_setup)
+
+/*
+ * CPU power down sequence as per A57/A53/A72 TRM
+ *
+ * x0 - L2 flush by HW(0) or SW(1), if system/HW driven L2 flush is supported
+ *
+ */
+#if IS_ENABLED(CONFIG_ARCH_ARM64_CORTEX_A57_POWER_DOWN_SUPPORT)
+ENTRY(cortex_a57_cpu_power_down)
+	/* Store L2 cache flush request */
+	mov	x13, x0
+
+	/* 1. Stop allocations to our data cache */
+	mrs	x0, sctlr_el1
+	bic	x0, x0, #1 << 2		// clear SCTLR.C
+	msr	sctlr_el1, x0
+	isb
+
+	mrs	x0, sctlr_el3
+	bic	x0, x0, #1 << 2		// clear SCTLR.C
+	msr	sctlr_el3, x0
+	isb
+
+	mrs	x0, midr_el1
+	ubfx	x0, x0, #4, #12
+	cmp	x0, #CORTEX_A53_PN
+	b.eq	a53
+
+	/* 2. Disable L2 prefetch */
+	mrs	x0, CPUECTLR_EL1	// CPUECTLR_EL1
+	/* CPUECTLR[38], disable table walk descriptor access L2 prefetch */
+	orr	x0, x0, #1 << 38
+	/*
+	 * CPUECTLR[36:35] L2 instruction fetch prefetch distance
+	 * 0 => disable instruction prefetch
+	 */
+	bic	x0, x0, #3 << 35
+	/*
+	 * CPUECTLR[33:32] L2 load/store prefetch distance
+	 * 0 => disable instruction prefetch
+	 */
+	bic	x0, x0, #3 << 32
+	msr	CPUECTLR_EL1, x0
+
+	/* 3. ISB to ensure ectlr write is complete */
+	isb
+
+	/* 4. DSB to ensure prior prefetches are complete */
+	dsb	sy
+
+a53:
+	/* 5. Clean and invalidate L1 and L2 if X13 == 1 */
+	mov	x0, #DCCISW
+	cmp	x13, #1
+	bne	1f
+	bl	flush_dcache_all
+	b	2f
+1:
+	bl	flush_dcache_louis
+2:
+
+	/* 6. Leave coherency, clear SMPEN */
+	mrs	x0, CPUECTLR_EL1
+	bic	x0, x0, #(1 << SMPEN_SHIFT)
+	msr	CPUECTLR_EL1, x0
+
+	/* 7. Set the DBGOSDLR.DLK, Double lock control bit */
+	mrs	x0, osdlr_el1
+	orr	x0, x0, #OSDLR_DBL_LOCK_BIT
+	msr	osdlr_el1, x0
+
+	/*
+	 * 9. Execute an ISB instruction to ensure that all of the
+	 * System register changes from the previous steps have
+	 * been committed.
+	 */
+	isb
+
+	/*
+	 * 10. Execute a DSB instruction to ensure that all
+	 * instruction cache, TLB, and branch predictor
+	 * maintenance operations issued by any processor in the
+	 * multiprocessor before the SMPEN bit was cleared have
+	 * completed.
+	 */
+	dsb	sy
+
+	/* 11. wfi */
+3:	wfi
+
+	/* we never return here */
+	b	3b
+ENDPROC(cortex_a57_cpu_power_down)
+#endif
diff --git a/src/arch/arm64/cpu/cortex_a57.h b/src/arch/arm64/cpu/cortex_a57.h
index 9e66f4d7ca..9e12e8b41f 100644
--- a/src/arch/arm64/cpu/cortex_a57.h
+++ b/src/arch/arm64/cpu/cortex_a57.h
@@ -23,4 +23,10 @@
 #define CPUECTLR_EL1	S3_1_c15_c2_1
 #define SMPEN_SHIFT	6
 
+/* Cortex MIDR[15:4] PN */
+#define CORTEX_A53_PN	0xd03
+
+/* Double lock control bit */
+#define OSDLR_DBL_LOCK_BIT	1
+
 #endif /* __ARCH_ARM64_CORTEX_A57_H__ */
diff --git a/src/arch/arm64/include/armv8/arch/cpu.h b/src/arch/arm64/include/armv8/arch/cpu.h
index 14635e39c0..4e15209be0 100644
--- a/src/arch/arm64/include/armv8/arch/cpu.h
+++ b/src/arch/arm64/include/armv8/arch/cpu.h
@@ -187,4 +187,18 @@ void arm64_cpu_startup_resume(void);
  */
 void arm64_arch_timer_init(void);
 
+/*
+ * The cortex_a57_cpu_power_down sequence as per A57/A53/A72 TRM.
+ * L2 flush by HW(0) or SW(1), if system/HW driven L2 flush is supported.
+ */
+#define NO_L2_FLUSH 0
+#define L2_FLUSH_HW 0
+#define L2_FLUSH_SW 1
+
+#if IS_ENABLED(CONFIG_ARCH_ARM64_CORTEX_A57_POWER_DOWN_SUPPORT)
+void cortex_a57_cpu_power_down(int l2_flush);
+#else
+static inline void cortex_a57_cpu_power_down(int l2_flush) {}
+#endif
+
 #endif /* __ARCH_CPU_H__ */