MLK15034-1: ARM: cpuidle imx7d: Extend imx_pen lock to cover entire flow

This makes the code much easier to reason about. In particular it o
makes sure the imx7d cpuidle driver respects the requirements for
cpu_cluster_pm_enter/exit:

* cpu_cluster_pm_enter must be called after cpu_pm_enter has been called
on all cpus in the power domain, and before cpu_pm_exit has been called
on any cpu in the power domain.
* cpu_cluster_pm_exit must be called after cpu_pm_enter has been called
on all cpus in the power domain, and before cpu_pm_exit has been called
on any cpu in the power domain.

This fixes interrupts sometimes getting "stuck" because of improper
save/restore of GIC DIST registers.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>
Signed-off-by: Leonard Crestez <leonard.crestez@nxp.com>
Reviewed-by: Anson Huang <Anson.Huang@nxp.com>

2 files changed, 49 insertions, 17 deletions

diff --git a/arch/arm/mach-imx/cpuidle-imx7d.c b/arch/arm/mach-imx/cpuidle-imx7d.c
index 9f72e11aac14..37d12b14134c 100644
--- a/arch/arm/mach-imx/cpuidle-imx7d.c
+++ b/arch/arm/mach-imx/cpuidle-imx7d.c
@@ -66,9 +66,9 @@ struct imx7_cpuidle_pm_info {
 	u32 ttbr;
 	u32 cpu1_wfi;
 	u32 lpi_enter;
-	u32 val;
-	u32 flag0;
-	u32 flag1;
+	atomic_t val;
+	atomic_t flag0;
+	atomic_t flag1;
 	struct imx7_pm_base ddrc_base;
 	struct imx7_pm_base ccm_base;
 	struct imx7_pm_base anatop_base;
@@ -84,6 +84,38 @@ static atomic_t master_wait = ATOMIC_INIT(0);
 static void (*imx7d_wfi_in_iram_fn)(void __iomem *iram_vbase);
 static struct imx7_cpuidle_pm_info *cpuidle_pm_info;
 
+static void imx_pen_lock(int cpu)
+{
+	if (cpu == 0) {
+		atomic_set(&cpuidle_pm_info->flag0, 1);
+		dsb();
+		atomic_set(&cpuidle_pm_info->val, cpu);
+		do {
+			dsb();
+		} while (atomic_read(&cpuidle_pm_info->flag1) == 1
+			&& atomic_read(&cpuidle_pm_info->val) == cpu)
+			;
+	} else {
+		atomic_set(&cpuidle_pm_info->flag1, 1);
+		dsb();
+		atomic_set(&cpuidle_pm_info->val, cpu);
+		do {
+			dsb();
+		} while (atomic_read(&cpuidle_pm_info->flag0) == 1
+			&& atomic_read(&cpuidle_pm_info->val) == cpu)
+			;
+	}
+}
+
+static void imx_pen_unlock(int cpu)
+{
+	dsb();
+	if (cpu == 0)
+		atomic_set(&cpuidle_pm_info->flag0, 0);
+	else
+		atomic_set(&cpuidle_pm_info->flag1, 0);
+}
+
 static int imx7d_idle_finish(unsigned long val)
 {
 	imx7d_wfi_in_iram_fn(wfi_iram_base);
@@ -105,6 +137,7 @@ static int imx7d_enter_low_power_idle(struct cpuidle_device *dev,
 		atomic_dec(&master_wait);
 		imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
 	} else {
+		imx_pen_lock(dev->cpu);
 		cpu_pm_enter();
 		if (atomic_inc_return(&master_lpi) == num_online_cpus() &&
 			cpuidle_pm_info->last_cpu == -1) {
@@ -113,20 +146,24 @@ static int imx7d_enter_low_power_idle(struct cpuidle_device *dev,
 			cpu_cluster_pm_enter();
 
 			cpuidle_pm_info->last_cpu = dev->cpu;
-			cpu_suspend(0, imx7d_idle_finish);
 
-			cpu_cluster_pm_exit();
-			imx_gpcv2_set_cpu_power_gate_in_idle(false);
-			/* initialize the last cpu id to invalid here */
-			cpuidle_pm_info->last_cpu = -1;
 		} else {
 			imx_set_cpu_jump(dev->cpu, ca7_cpu_resume);
-			cpu_suspend(0, imx7d_idle_finish);
 		}
-		atomic_dec(&master_lpi);
+
+		cpu_suspend(0, imx7d_idle_finish);
+
+		if (atomic_dec_return(&master_lpi) == (num_online_cpus() - 1)) {
+			cpu_cluster_pm_exit();
+			imx_gpcv2_set_cpu_power_gate_in_idle(false);
+			imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
+		}
+
+		if (cpuidle_pm_info->last_cpu == dev->cpu)
+			cpuidle_pm_info->last_cpu = -1;
 
 		cpu_pm_exit();
-		imx_gpcv2_set_lpm_mode(WAIT_CLOCKED);
+		imx_pen_unlock(dev->cpu);
 	}
 
 	return index;
diff --git a/arch/arm/mach-imx/imx7d_low_power_idle.S b/arch/arm/mach-imx/imx7d_low_power_idle.S
index 59cf4f4d7150..72a2ce728c43 100644
--- a/arch/arm/mach-imx/imx7d_low_power_idle.S
+++ b/arch/arm/mach-imx/imx7d_low_power_idle.S
@@ -612,8 +612,6 @@ ENTRY(imx7d_low_power_idle)
 
 	tlb_set_to_ocram
 
-	imx_pen_lock
-
 	ldr	r6, [r0, #PM_INFO_PM_INFO_LAST_CPU_OFFSET]
 	cmp	r11, r6
 	bne	first_cpu
@@ -748,13 +746,12 @@ do_exit_wfi:
 	ldr	r7, =0x1
 	ldr	r8, =0x1000
 	str	r7, [r10, r8]
+
 	/* clear lpi enter flag */
 	ldr	r7, =0x0
 	str	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
 skip_lpi_flow:
 
-	imx_pen_unlock
-
 	tlb_back_to_ddr
 
 #ifdef CONFIG_SMP
@@ -816,8 +813,6 @@ do_exit_lp_idle:
 	str	r7, [r0, #PM_INFO_PM_INFO_LPI_ENTER_OFFSET]
 wakeup_skip_lpi_flow:
 
-	imx_pen_unlock
-
 	/* get physical resume address from pm_info. */
 	ldr	lr, [r0, #PM_INFO_RESUME_ADDR_OFFSET]