MLK-16750-2 arm: imx: busfreq: lock L2 cache instead of disabling it

In non-secure mode, L2 cache can NOT be disabled, lock
L2 cache instead of disabling it to avoid L2 cache
access DDR.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>
Signed-off-by: Peng Fan<peng.fan@nxp.com>
Reviewed-by: Anson Huang <Anson.Huang@nxp.com>

4 files changed, 119 insertions, 22 deletions

diff --git a/arch/arm/mach-imx/ddr3_freq_imx6.S b/arch/arm/mach-imx/ddr3_freq_imx6.S
index 2a12669a3bef..e6f7f74f7d32 100644
--- a/arch/arm/mach-imx/ddr3_freq_imx6.S
+++ b/arch/arm/mach-imx/ddr3_freq_imx6.S
@@ -39,6 +39,13 @@
 #define CCM_CDHIPR			0x48
 
 #define L2_CACHE_SYNC		0x730
+#define PL310_AUX_CTRL          0x104
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_AUX_16WAY_BIT	0x10000
+#define PL310_LOCKDOWN_NBREGS   8
+#define PL310_LOCKDOWN_SZREG    4
+#define PL310_8WAYS_MASK        0x00FF
+#define PL310_16WAYS_UPPERMASK  0xFF00
 
 #define IMX6QP_REVISION_ID 0x630100
 #define ANADIG_DIGPROG     0x260
@@ -422,11 +429,20 @@ wait_for_l2_to_idle:
 	mov	r1, #0x0
 	str	r1, [r12, #L2_CACHE_SYNC]
 
-	/* Disable L2. */
-	str	r1, [r12, #0x100]
-
 	dsb
 	isb
+
+	ldr	r1, [r12, #PL310_AUX_CTRL]
+	tst	r1, #PL310_AUX_16WAY_BIT
+	mov	r1, #PL310_8WAYS_MASK
+	orrne	r1, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r12, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 	/*
@@ -1022,10 +1038,18 @@ done:
 	str	r0, [r5, #MMDC0_MAPSR]
 
 #ifdef CONFIG_CACHE_L2X0
-	/* Enable L2. */
-	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
-	ldr	r6, =0x1
-	str	r6, [r7, #0x100]
+	ldr	r1, [r12, #PL310_AUX_CTRL]
+	tst	r1, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r1, #0x00	/* 8 ways mask */
+	orrne	r1, #0x0000	/* 16 ways mask */
+	add	r5, r12, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	str	r1, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
+
 	isb
 	dsb
 #endif
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6.S b/arch/arm/mach-imx/lpddr2_freq_imx6.S
index a7f387701bfa..21179fb2cb72 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6.S
@@ -15,6 +15,14 @@
 #include <linux/linkage.h>
 #include "hardware.h"
 
+#define PL310_AUX_CTRL          0x104
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_AUX_16WAY_BIT	0x10000
+#define PL310_LOCKDOWN_NBREGS   8
+#define PL310_LOCKDOWN_SZREG    4
+#define PL310_8WAYS_MASK        0x00FF
+#define PL310_16WAYS_UPPERMASK  0xFF00
+
 .globl imx6_lpddr2_freq_change_start
 .globl imx6_lpddr2_freq_change_end
 
@@ -409,8 +417,17 @@ wait_for_l2_to_idle:
 	dsb
 	isb
 
-	/* Disable L2. */
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r3, #PL310_8WAYS_MASK
+	orrne	r3, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 	ldr	r3, =IMX_IO_P2V(MX6Q_ANATOP_BASE_ADDR)
@@ -511,10 +528,18 @@ skip_power_down:
 	str	r6, [r8, #0x410]
 
 #ifdef CONFIG_CACHE_L2X0
-	/* Enable L2. */
 	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
-	ldr	r6, =0x1
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r3, #0x00	/* 8 ways mask */
+	orrne	r3, #0x0000	/* 16 ways mask */
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 	/* Enable L1 data cache. */
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6sll.S b/arch/arm/mach-imx/lpddr2_freq_imx6sll.S
index edc115c6d72e..c67d9e2b82ef 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6sll.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6sll.S
@@ -21,6 +21,13 @@
 #define CCM_CDHIPR	0x48
 
 #define L2_CACHE_SYNC	0x730
+#define PL310_AUX_CTRL          0x104
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_AUX_16WAY_BIT	0x10000
+#define PL310_LOCKDOWN_NBREGS   8
+#define PL310_LOCKDOWN_SZREG    4
+#define PL310_8WAYS_MASK        0x00FF
+#define PL310_16WAYS_UPPERMASK  0xFF00
 
 #define MMDC0_MDPDC	0x4
 #define MMDC0_MAPSR	0x404
@@ -269,8 +276,17 @@ ENTRY(imx6sll_lpddr2_freq_change)
 	dsb
 	isb
 
-	/* Disable L2. */
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r3, #PL310_8WAYS_MASK
+	orrne	r3, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 	ldr	r2, =IMX_IO_P2V(MX6Q_CCM_BASE_ADDR)
@@ -363,10 +379,18 @@ poll_dvfs_clear_1:
 
 
 #ifdef CONFIG_CACHE_L2X0
-	/* Enable L2. */
 	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
-	ldr	r6, =0x1
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r3, #0x00	/* 8 ways mask */
+	orrne	r3, #0x0000	/* 16 ways mask */
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 	/* Enable L1 data cache. */
diff --git a/arch/arm/mach-imx/lpddr2_freq_imx6sx.S b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
index 31555347b82f..ba3488cad9d4 100644
--- a/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
+++ b/arch/arm/mach-imx/lpddr2_freq_imx6sx.S
@@ -21,6 +21,13 @@
 #define CCM_CDHIPR	0x48
 
 #define L2_CACHE_SYNC	0x730
+#define PL310_AUX_CTRL          0x104
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_AUX_16WAY_BIT	0x10000
+#define PL310_LOCKDOWN_NBREGS   8
+#define PL310_LOCKDOWN_SZREG    4
+#define PL310_8WAYS_MASK        0x00FF
+#define PL310_16WAYS_UPPERMASK  0xFF00
 
 #define MMDC0_MDPDC	0x4
 #define MMDC0_MAPSR	0x404
@@ -285,8 +292,17 @@ ENTRY(imx6_up_lpddr2_freq_change)
 	dsb
 	isb
 
-	/* Disable L2. */
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r3, #PL310_8WAYS_MASK
+	orrne	r3, #PL310_16WAYS_UPPERMASK
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 skip_disable_l2:
@@ -394,10 +410,18 @@ skip_power_down:
 	beq	skip_enable_l2
 
 #ifdef CONFIG_CACHE_L2X0
-	/* Enable L2. */
 	ldr	r7, =IMX_IO_P2V(MX6Q_L2_BASE_ADDR)
-	ldr	r6, =0x1
-	str	r6, [r7, #0x100]
+	ldr	r3, [r7, #PL310_AUX_CTRL]
+	tst	r3, #PL310_AUX_16WAY_BIT
+	mov	r6, #PL310_LOCKDOWN_NBREGS
+	mov	r3, #0x00	/* 8 ways mask */
+	orrne	r3, #0x0000	/* 16 ways mask */
+	add	r5, r7, #PL310_DCACHE_LOCKDOWN_BASE
+1:	/* lock Dcache and Icache */
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	str	r3, [r5], #PL310_LOCKDOWN_SZREG
+	subs	r6, r6, #1
+	bne	1b
 #endif
 
 skip_enable_l2: