MLK-12023-2: arm: imx6: refactor wfe ddr3 freq change low power mode

Refactor wfe_ddr3_freq_change to wfe_smp_freq_change in order
to reuse enter/exit wfe standby low power mode.

Signed-off-by: Adrian Alonso <aalonso@freescale.com>
Signed-off-by: Anson Huang <b20788@freescale.com>
(cherry picked from commit 4d92e1f5bab3480cb145821dce27960887e7a3ea)

5 files changed, 142 insertions, 89 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index beb45f2d4c34..432be7d8e609 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -98,7 +98,8 @@ obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 endif
 obj-y += busfreq_lpddr2.o busfreq-imx.o busfreq_ddr3.o
 AFLAGS_ddr3_freq_imx6.o :=-Wa,-march=armv7-a
-obj-$(CONFIG_SOC_IMX6Q) += clk-imx6q.o mach-imx6q.o ddr3_freq_imx6.o
+AFLAGS_smp_wfe_imx6.o :=-Wa,-march=armv7-a
+obj-$(CONFIG_SOC_IMX6Q) += clk-imx6q.o mach-imx6q.o ddr3_freq_imx6.o smp_wfe_imx6.o
 AFLAGS_lpddr2_freq_imx6.o :=-Wa,-march=armv7-a
 obj-$(CONFIG_SOC_IMX6SL) += clk-imx6sl.o mach-imx6sl.o lpddr2_freq_imx6.o
 AFLAGS_ddr3_freq_imx6sx.o :=-Wa,-march=armv7-a
diff --git a/arch/arm/mach-imx/busfreq_ddr3.c b/arch/arm/mach-imx/busfreq_ddr3.c
index e53d6212b423..37281158fbff 100644
--- a/arch/arm/mach-imx/busfreq_ddr3.c
+++ b/arch/arm/mach-imx/busfreq_ddr3.c
@@ -122,11 +122,10 @@ static u32 *irqs_used;
 
 void (*wfe_change_ddr_freq)(u32 cpuid, u32 *ddr_freq_change_done);
 void (*imx7_wfe_change_ddr_freq)(u32 cpuid, u32 ocram_base);
-extern void wfe_ddr3_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
+extern void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done);
 extern void imx7_smp_wfe(u32 cpuid, u32 ocram_base);
-extern unsigned long wfe_ddr3_freq_change_start
-	asm("wfe_ddr3_freq_change_start");
-extern unsigned long wfe_ddr3_freq_change_end asm("wfe_ddr3_freq_change_end");
+extern unsigned long wfe_smp_freq_change_start asm("wfe_smp_freq_change_start");
+extern unsigned long wfe_smp_freq_change_end asm("wfe_smp_freq_change_end");
 extern void __iomem *imx_scu_base;
 #endif
 
@@ -720,11 +719,11 @@ int init_mmdc_ddr3_settings_imx6_smp(struct platform_device *busfreq_pdev)
 			wfe_freq_change_iram_base += FNCPY_ALIGN -
 			((uintptr_t)wfe_freq_change_iram_base % (FNCPY_ALIGN));
 
-	wfe_code_size = (&wfe_ddr3_freq_change_end -
-		&wfe_ddr3_freq_change_start) *4;
+	wfe_code_size = (&wfe_smp_freq_change_end -
+		&wfe_smp_freq_change_start) *4;
 
 	wfe_change_ddr_freq = (void *)fncpy((void *)wfe_freq_change_iram_base,
-		&wfe_ddr3_freq_change, wfe_code_size);
+		&wfe_smp_freq_change, wfe_code_size);
 
 	/*
 	 * Store the variable used to communicate
diff --git a/arch/arm/mach-imx/common.c b/arch/arm/mach-imx/common.c
index 82f73884101c..397a2e7dfc7e 100644
--- a/arch/arm/mach-imx/common.c
+++ b/arch/arm/mach-imx/common.c
@@ -135,10 +135,10 @@ void imx6_up_lpddr2_freq_change(u32 freq, int bus_freq_mode) {}
 
 #if !defined(CONFIG_SOC_IMX6Q)
 u32 mx6_ddr3_freq_change_start, mx6_ddr3_freq_change_end;
-u32 wfe_ddr3_freq_change_start, wfe_ddr3_freq_change_end;
+u32 wfe_smp_freq_change_start, wfe_smp_freq_change_end;
 void mx6_ddr3_freq_change(u32 freq, void *ddr_settings,
 	bool dll_mode, void *iomux_offsets) {}
-void wfe_ddr3_freq_change(u32 cpuid, u32 *ddr_freq_change_done) {}
+void wfe_smp_freq_change(u32 cpuid, u32 *ddr_freq_change_done) {}
 #endif
 
 #if !defined(CONFIG_SOC_IMX7D)
diff --git a/arch/arm/mach-imx/ddr3_freq_imx6.S b/arch/arm/mach-imx/ddr3_freq_imx6.S
index 9e503fff80ba..ddac7f1284bf 100644
--- a/arch/arm/mach-imx/ddr3_freq_imx6.S
+++ b/arch/arm/mach-imx/ddr3_freq_imx6.S
@@ -44,14 +44,9 @@
 #define ANADIG_DIGPROG     0x260
 
 .extern iram_tlb_phys_addr
-#ifdef CONFIG_SMP
-.extern imx_scu_base
-#endif
 
 .globl mx6_ddr3_freq_change_start
 .globl mx6_ddr3_freq_change_end
-.globl wfe_ddr3_freq_change_start
-.globl wfe_ddr3_freq_change_end
 
 	.align 3
 
@@ -1080,77 +1075,3 @@ done:
 	 */
 	.ltorg
 mx6_ddr3_freq_change_end:
-
-#ifdef CONFIG_SMP
-	.align 3
-
-ENTRY(wfe_ddr3_freq_change)
-wfe_ddr3_freq_change_start:
-	push	{r4 - r11, lr}
-
-	mov	r6, r0
-	mov	r7, r1
-
-	dsb
-	isb
-
-	disable_l1_dcache
-
-	isb
-
-	/* Turn off SMP bit. */
-	mrc	p15, 0, r8, c1, c0, 1
-	bic	r8, r8, #0x40
-	mcr	p15, 0, r8, c1, c0, 1
-
-	isb
-
-	/* Inform the SCU we are going to enter WFE. */
-	push	{r0 - r11, lr}
-
-	ldr	r0,=imx_scu_base
-	ldr	r0, [r0]
-	mov	r1, #SCU_PM_DORMANT
-	ldr	r3, =scu_power_mode
-	mov	lr, pc
-	mov	pc, r3
-
-	pop	{r0 - r11, lr}
-
-go_back_wfe:
-	wfe
-
-	ldr	r3, [r7]
-	cmp	r3, #1
-	beq	go_back_wfe
-
-	/* Turn ON SMP bit. */
-	mrc	p15, 0, r8, c1, c0, 1
-	orr	r8, r8, #0x40
-	mcr	p15, 0, r8, c1, c0, 1
-
-	isb
-	/* Enable L1 data cache. */
-	mrc	p15, 0, r8, c1, c0, 0
-	orr	r8, r8, #0x4
-	mcr	p15, 0, r8, c1, c0, 0
-	isb
-
-	/* Inform the SCU we have exited WFE. */
-	push	{r0 - r11, lr}
-
-	ldr	r0,=imx_scu_base
-	ldr	r0, [r0]
-	mov	r1, #SCU_PM_NORMAL
-	ldr	r3, =scu_power_mode
-	mov	lr, pc
-	mov	pc, r3
-
-	pop	{r0 - r11, lr}
-
-	/* Pop all saved registers. */
-	pop	{r4 - r11, lr}
-	mov	pc, lr
-	.ltorg
-wfe_ddr3_freq_change_end:
-#endif
diff --git a/arch/arm/mach-imx/smp_wfe_imx6.S b/arch/arm/mach-imx/smp_wfe_imx6.S
new file mode 100644
index 000000000000..cdee6169958e
--- /dev/null
+++ b/arch/arm/mach-imx/smp_wfe_imx6.S
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/linkage.h>
+#include <asm/smp_scu.h>
+#include "hardware.h"
+
+#ifdef CONFIG_SMP
+.extern imx_scu_base
+#endif
+
+.globl wfe_smp_freq_change_start
+.globl wfe_smp_freq_change_end
+
+#ifdef CONFIG_SMP
+
+	.align 3
+
+	.macro	disable_l1_dcache
+
+	/*
+	 * Flush all data from the L1 data cache before disabling
+	 * SCTLR.C bit.
+	 */
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	/* disable d-cache */
+	mrc	p15, 0, r6, c1, c0, 0
+	bic	r6, r6, #0x4
+	mcr	p15, 0, r6, c1, c0, 0
+	dsb
+	isb
+
+	push	{r0 - r11, lr}
+
+	ldr	r7, =v7_flush_kern_cache_all
+	mov	lr, pc
+	mov	pc, r7
+	pop	{r0 - r11, lr}
+
+	.endm
+
+ENTRY(wfe_smp_freq_change)
+wfe_smp_freq_change_start:
+	push	{r4 - r11, lr}
+
+	mov	r6, r0
+	mov	r7, r1
+
+	dsb
+	isb
+
+	disable_l1_dcache
+
+	isb
+
+	/* Turn off SMP bit. */
+	mrc	p15, 0, r8, c1, c0, 1
+	bic	r8, r8, #0x40
+	mcr	p15, 0, r8, c1, c0, 1
+
+	isb
+
+	/* Inform the SCU we are going to enter WFE. */
+	push	{r0 - r11, lr}
+
+	ldr	r0,=imx_scu_base
+	ldr	r0, [r0]
+	mov	r1, #SCU_PM_DORMANT
+	ldr	r3, =scu_power_mode
+	mov	lr, pc
+	mov	pc, r3
+
+	pop	{r0 - r11, lr}
+
+go_back_wfe:
+	wfe
+
+	ldr	r3, [r7]
+	cmp	r3, #1
+	beq	go_back_wfe
+
+	/* Turn ON SMP bit. */
+	mrc	p15, 0, r8, c1, c0, 1
+	orr	r8, r8, #0x40
+	mcr	p15, 0, r8, c1, c0, 1
+
+	isb
+	/* Enable L1 data cache. */
+	mrc	p15, 0, r8, c1, c0, 0
+	orr	r8, r8, #0x4
+	mcr	p15, 0, r8, c1, c0, 0
+	isb
+
+	/* Inform the SCU we have exited WFE. */
+	push	{r0 - r11, lr}
+
+	ldr	r0,=imx_scu_base
+	ldr	r0, [r0]
+	mov	r1, #SCU_PM_NORMAL
+	ldr	r3, =scu_power_mode
+	mov	lr, pc
+	mov	pc, r3
+
+	pop	{r0 - r11, lr}
+
+	/* Pop all saved registers. */
+	pop	{r4 - r11, lr}
+	mov	pc, lr
+	.ltorg
+wfe_smp_freq_change_end:
+#endif