1 files changed, 61 insertions, 12 deletions

diff --git a/arch/arm/mach-socfpga/misc.c b/arch/arm/mach-socfpga/misc.c
index ec8339e0457..49dadd4c3d8 100644
--- a/arch/arm/mach-socfpga/misc.c
+++ b/arch/arm/mach-socfpga/misc.c
@@ -48,10 +48,10 @@ int dram_init(void)
 
 void enable_caches(void)
 {
-#ifndef CONFIG_SYS_ICACHE_OFF
+#if !CONFIG_IS_ENABLED(SYS_ICACHE_OFF)
 	icache_enable();
 #endif
-#ifndef CONFIG_SYS_DCACHE_OFF
+#if !CONFIG_IS_ENABLED(SYS_DCACHE_OFF)
 	dcache_enable();
 #endif
 }
@@ -59,6 +59,24 @@ void enable_caches(void)
 #ifdef CONFIG_SYS_L2_PL310
 void v7_outer_cache_enable(void)
 {
+	struct udevice *dev;
+
+	if (uclass_get_device(UCLASS_CACHE, 0, &dev))
+		pr_err("cache controller driver NOT found!\n");
+}
+
+void v7_outer_cache_disable(void)
+{
+	/* Disable the L2 cache */
+	clrbits_le32(&pl310->pl310_ctrl, L2X0_CTRL_EN);
+}
+
+void socfpga_pl310_clear(void)
+{
+	u32 mask = 0xff, ena = 0;
+
+	icache_enable();
+
 	/* Disable the L2 cache */
 	clrbits_le32(&pl310->pl310_ctrl, L2X0_CTRL_EN);
 
@@ -72,11 +90,37 @@ void v7_outer_cache_enable(void)
 		     L310_SHARED_ATT_OVERRIDE_ENABLE);
 
 	/* Enable the L2 cache */
-	setbits_le32(&pl310->pl310_ctrl, L2X0_CTRL_EN);
-}
+	ena = readl(&pl310->pl310_ctrl);
+	ena |= L2X0_CTRL_EN;
+
+	/*
+	 * Invalidate the PL310 L2 cache. Keep the invalidation code
+	 * entirely in L1 I-cache to avoid any bus traffic through
+	 * the L2.
+	 */
+	asm volatile(
+		".align	5			\n"
+		"	b	3f		\n"
+		"1:	str	%1,	[%4]	\n"
+		"	dsb			\n"
+		"	isb			\n"
+		"	str	%0,	[%2]	\n"
+		"	dsb			\n"
+		"	isb			\n"
+		"2:	ldr	%0,	[%2]	\n"
+		"	cmp	%0,	#0	\n"
+		"	bne	2b		\n"
+		"	str	%0,	[%3]	\n"
+		"	dsb			\n"
+		"	isb			\n"
+		"	b	4f		\n"
+		"3:	b	1b		\n"
+		"4:	nop			\n"
+	: "+r"(mask), "+r"(ena)
+	: "r"(&pl310->pl310_inv_way),
+	  "r"(&pl310->pl310_cache_sync), "r"(&pl310->pl310_ctrl)
+	: "memory", "cc");
 
-void v7_outer_cache_disable(void)
-{
 	/* Disable the L2 cache */
 	clrbits_le32(&pl310->pl310_ctrl, L2X0_CTRL_EN);
 }
@@ -126,17 +170,22 @@ int arch_cpu_init(void)
 #ifndef CONFIG_SPL_BUILD
 static int do_bridge(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
-	if (argc != 2)
+	unsigned int mask = ~0;
+
+	if (argc < 2 || argc > 3)
 		return CMD_RET_USAGE;
 
 	argv++;
 
+	if (argc == 3)
+		mask = simple_strtoul(argv[1], NULL, 16);
+
 	switch (*argv[0]) {
 	case 'e':	/* Enable */
-		do_bridge_reset(1);
+		do_bridge_reset(1, mask);
 		break;
 	case 'd':	/* Disable */
-		do_bridge_reset(0);
+		do_bridge_reset(0, mask);
 		break;
 	default:
 		return CMD_RET_USAGE;
@@ -145,10 +194,10 @@ static int do_bridge(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	return 0;
 }
 
-U_BOOT_CMD(bridge, 2, 1, do_bridge,
+U_BOOT_CMD(bridge, 3, 1, do_bridge,
 	   "SoCFPGA HPS FPGA bridge control",
-	   "enable  - Enable HPS-to-FPGA, FPGA-to-HPS, LWHPS-to-FPGA bridges\n"
-	   "bridge disable - Enable HPS-to-FPGA, FPGA-to-HPS, LWHPS-to-FPGA bridges\n"
+	   "enable [mask] - Enable HPS-to-FPGA, FPGA-to-HPS, LWHPS-to-FPGA bridges\n"
+	   "bridge disable [mask] - Enable HPS-to-FPGA, FPGA-to-HPS, LWHPS-to-FPGA bridges\n"
 	   ""
 );