From 13e82edf2e78eae08952cfbaff58bd4db0a5a0da Mon Sep 17 00:00:00 2001
From: "fushan.zeng" <fushan.zeng@amlogic.com>
Date: Thu, 19 Nov 2020 13:32:02 +0800
Subject: [PATCH] linux: reboot power down cpu timeout [1/3]

PD#SH-5853

Problem:
reboot power down cpu timeout

Solution:
power down cpus throug bl31
set PWRN_EN bits

Verify:
S905D3

Signed-off-by: fushan.zeng <fushan.zeng@amlogic.com>
Change-Id: Ib0a9bcf5ef4d0115ba0e83864f6497761050d6f5
---
 arch/arm/kernel/smp.c   | 17 +++++++++++++++++
 arch/arm64/kernel/smp.c | 15 +++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index 3f9ce5ab96cd..30bbc1961d03 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -48,6 +48,10 @@
 #include <asm/mach/arch.h>
 #include <asm/mpu.h>
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+#include <asm/cp15.h>
+#define MIDR		__ACCESS_CP15(c0, 0, c0, 0)
+#endif
 #define CREATE_TRACE_POINTS
 #include <trace/events/ipi.h>
 
@@ -575,6 +579,19 @@ static void ipi_cpu_stop(unsigned int cpu)
 	local_fiq_disable();
 	local_irq_disable();
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+	/* CORTEX-A55 need power down here for shutdown*/
+	/* If A55 enter WFI here, it is possible quit from wfi,
+	 *  which cause CPU PACTIVE check fail.
+	 */
+#ifdef CONFIG_HOTPLUG_CPU
+	if (((read_cpuid_id() >> 4) & 0xFFF) == 0xD05) {
+		flush_cache_louis();
+		if (smp_ops.cpu_die)
+			smp_ops.cpu_die(cpu);
+	}
+#endif
+#endif
 	while (1)
 		cpu_relax();
 }
diff --git a/arch/arm64/kernel/smp.c b/arch/arm64/kernel/smp.c
index d034e17211ae..93acad5adbc4 100644
--- a/arch/arm64/kernel/smp.c
+++ b/arch/arm64/kernel/smp.c
@@ -62,6 +62,9 @@
 #define CREATE_TRACE_POINTS
 #include <trace/events/ipi.h>
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+#include <asm/cputype.h>
+#endif
 DEFINE_PER_CPU_READ_MOSTLY(int, cpu_number);
 EXPORT_PER_CPU_SYMBOL(cpu_number);
 
@@ -833,6 +836,18 @@ static void ipi_cpu_stop(unsigned int cpu)
 
 	local_irq_disable();
 
+#ifdef CONFIG_AMLOGIC_MODIFY
+	/* CORTEX-A55 need power down here for shutdown*/
+	/* If A55 enter WFI here, it is possible quit from wfi,
+	 *  which cause CPU PACTIVE check fail.
+	 */
+#ifdef CONFIG_HOTPLUG_CPU
+	if ((read_cpuid_id() & MIDR_CPU_MODEL_MASK) == MIDR_CORTEX_A55) {
+		if (cpu_ops[cpu] && cpu_ops[cpu]->cpu_die)
+			cpu_ops[cpu]->cpu_die(cpu);
+	}
+#endif
+#endif
 	while (1)
 		cpu_relax();
 }