From 8e1cc44900d31dd60ad6c5e3853f9df6ea9826e9 Mon Sep 17 00:00:00 2001
From: Chandni Cherukuri <chandni.cherukuri@arm.com>
Date: Thu, 2 Aug 2018 12:29:07 +0530
Subject: [PATCH] sgi: disable CPU power down bit in reset handler

On SGI platforms, the 'CORE_PWRDN_EN' bit of 'CPUPWRCTLR_EL1'
register requires an explicit write to clear it for hotplug and
idle to function correctly. The reset value of this bit is zero
but it still requires this explicit clear to zero. This indicates
that this could be a model related issue but for now this issue can
be fixed be clearing the CORE_PWRDN_EN in the platform specific
reset handler function.

Change-Id: I4222930daa9a3abacdace6b7c3f4a5472ac0cb19
Signed-off-by: Chandni Cherukuri <chandni.cherukuri@arm.com>
---
 plat/arm/css/sgi/aarch64/sgi_helper.S | 40 +++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/plat/arm/css/sgi/aarch64/sgi_helper.S b/plat/arm/css/sgi/aarch64/sgi_helper.S
index aaa51560f..d6f63ede9 100644
--- a/plat/arm/css/sgi/aarch64/sgi_helper.S
+++ b/plat/arm/css/sgi/aarch64/sgi_helper.S
@@ -7,9 +7,11 @@
 #include <arch.h>
 #include <asm_macros.S>
 #include <platform_def.h>
+#include <cortex_a75.h>
 
 	.globl	plat_is_my_cpu_primary
 	.globl	plat_arm_calc_core_pos
+	.globl	plat_reset_handler
 
 	/* -----------------------------------------------------
 	 * unsigned int plat_is_my_cpu_primary (void);
@@ -65,3 +67,41 @@ func plat_arm_calc_core_pos
 	madd    x0, x1, x5, x0
 	ret
 endfunc plat_arm_calc_core_pos
+
+	/* ------------------------------------------------------
+	 * Helper macro that reads the part number of the current
+	 * CPU and jumps to the given label if it matches the CPU
+	 * MIDR provided.
+	 *
+	 * Clobbers x0.
+	 * -----------------------------------------------------
+	 */
+	.macro  jump_if_cpu_midr _cpu_midr, _label
+	mrs	x0, midr_el1
+	ubfx	x0, x0, MIDR_PN_SHIFT, #12
+	cmp	w0, #((\_cpu_midr >> MIDR_PN_SHIFT) & MIDR_PN_MASK)
+	b.eq	\_label
+	.endm
+
+	/* -----------------------------------------------------
+	 * void plat_reset_handler(void);
+	 *
+	 * Determine the CPU MIDR and disable power down bit for
+	 * that CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_reset_handler
+	jump_if_cpu_midr CORTEX_A75_MIDR, A75
+	ret
+
+	/* -----------------------------------------------------
+	 * Disable CPU power down bit in power control register
+	 * -----------------------------------------------------
+	 */
+A75:
+	mrs	x0, CORTEX_A75_CPUPWRCTLR_EL1
+	bic	x0, x0, #CORTEX_A75_CORE_PWRDN_EN_MASK
+	msr	CORTEX_A75_CPUPWRCTLR_EL1, x0
+	isb
+	ret
+endfunc plat_reset_handler