1 files changed, 50 insertions, 5 deletions

diff --git a/drivers/reset/reset-socfpga.c b/drivers/reset/reset-socfpga.c
index b2acfcd2ec..cb8312619f 100644
--- a/drivers/reset/reset-socfpga.c
+++ b/drivers/reset/reset-socfpga.c
@@ -24,9 +24,39 @@
 #define NR_BANKS		8
 
 struct socfpga_reset_data {
-	void __iomem *membase;
+	void __iomem *modrst_base;
 };
 
+/*
+ * For compatibility with Kernels that don't support peripheral reset, this
+ * driver can keep the old behaviour of not asserting peripheral reset before
+ * starting the OS and deasserting all peripheral resets (enabling all
+ * peripherals).
+ *
+ * For that, the reset driver checks the environment variable
+ * "socfpga_legacy_reset_compat". If this variable is '1', perihperals are not
+ * reset again once taken out of reset and all peripherals in 'permodrst' are
+ * taken out of reset before booting into the OS.
+ * Note that this should be required for gen5 systems only that are running
+ * Linux kernels without proper peripheral reset support for all drivers used.
+ */
+static bool socfpga_reset_keep_enabled(void)
+{
+#if !defined(CONFIG_SPL_BUILD) || CONFIG_IS_ENABLED(ENV_SUPPORT)
+	const char *env_str;
+	long val;
+
+	env_str = env_get("socfpga_legacy_reset_compat");
+	if (env_str) {
+		val = simple_strtol(env_str, NULL, 0);
+		if (val == 1)
+			return true;
+	}
+#endif
+
+	return false;
+}
+
 static int socfpga_reset_assert(struct reset_ctl *reset_ctl)
 {
 	struct socfpga_reset_data *data = dev_get_priv(reset_ctl->dev);
@@ -35,7 +65,7 @@ static int socfpga_reset_assert(struct reset_ctl *reset_ctl)
 	int bank = id / (reg_width * BITS_PER_BYTE);
 	int offset = id % (reg_width * BITS_PER_BYTE);
 
-	setbits_le32(data->membase + (bank * BANK_INCREMENT), BIT(offset));
+	setbits_le32(data->modrst_base + (bank * BANK_INCREMENT), BIT(offset));
 	return 0;
 }
 
@@ -47,7 +77,7 @@ static int socfpga_reset_deassert(struct reset_ctl *reset_ctl)
 	int bank = id / (reg_width * BITS_PER_BYTE);
 	int offset = id % (reg_width * BITS_PER_BYTE);
 
-	clrbits_le32(data->membase + (bank * BANK_INCREMENT), BIT(offset));
+	clrbits_le32(data->modrst_base + (bank * BANK_INCREMENT), BIT(offset));
 	return 0;
 }
 
@@ -80,11 +110,24 @@ static int socfpga_reset_probe(struct udevice *dev)
 	const void *blob = gd->fdt_blob;
 	int node = dev_of_offset(dev);
 	u32 modrst_offset;
+	void __iomem *membase;
 
-	data->membase = devfdt_get_addr_ptr(dev);
+	membase = devfdt_get_addr_ptr(dev);
 
 	modrst_offset = fdtdec_get_int(blob, node, "altr,modrst-offset", 0x10);
-	data->membase += modrst_offset;
+	data->modrst_base = membase + modrst_offset;
+
+	return 0;
+}
+
+static int socfpga_reset_remove(struct udevice *dev)
+{
+	struct socfpga_reset_data *data = dev_get_priv(dev);
+
+	if (socfpga_reset_keep_enabled()) {
+		puts("Deasserting all peripheral resets\n");
+		writel(0, data->modrst_base + 4);
+	}
 
 	return 0;
 }
@@ -101,4 +144,6 @@ U_BOOT_DRIVER(socfpga_reset) = {
 	.probe = socfpga_reset_probe,
 	.priv_auto_alloc_size = sizeof(struct socfpga_reset_data),
 	.ops = &socfpga_reset_ops,
+	.remove = socfpga_reset_remove,
+	.flags	= DM_FLAG_OS_PREPARE,
 };