2 files changed, 108 insertions, 0 deletions

diff --git a/arch/arm/mach-keystone/include/mach/psc_defs.h b/arch/arm/mach-keystone/include/mach/psc_defs.h
index 2202946be5..6e6e7fd433 100644
--- a/arch/arm/mach-keystone/include/mach/psc_defs.h
+++ b/arch/arm/mach-keystone/include/mach/psc_defs.h
@@ -56,6 +56,8 @@
 #define PSC_REG_MDSTAT_GET_STATUS(x)        BOOT_READ_BITFIELD((x), 5, 0)
 #define PSC_REG_MDSTAT_GET_LRSTZ(x)         BOOT_READ_BITFIELD((x), 8, 8)
 #define PSC_REG_MDSTAT_GET_LRSTDONE(x)      BOOT_READ_BITFIELD((x), 9, 9)
+#define PSC_REG_MDSTAT_GET_MRSTZ(x)         BOOT_READ_BITFIELD((x), 10, 10)
+#define PSC_REG_MDSTAT_GET_MRSTDONE(x)      BOOT_READ_BITFIELD((x), 11, 11)
 
 /* PDCTL states */
 #define PSC_REG_VAL_PDCTL_NEXT_ON           1
@@ -86,5 +88,7 @@ u32 psc_get_domain_num(u32 mod_num);
 int psc_enable_module(u32 mod_num);
 int psc_disable_module(u32 mod_num);
 int psc_disable_domain(u32 domain_num);
+int psc_module_keep_in_reset_enabled(u32 mod_num, bool gate_clocks);
+int psc_module_release_from_reset(u32 mod_num);
 
 #endif /* _PSC_DEFS_H_ */
diff --git a/arch/arm/mach-keystone/psc.c b/arch/arm/mach-keystone/psc.c
index 8991e23773..ff042a6db2 100644
--- a/arch/arm/mach-keystone/psc.c
+++ b/arch/arm/mach-keystone/psc.c
@@ -233,3 +233,107 @@ int psc_disable_domain(u32 domain_num)
 
 	return psc_wait(domain_num);
 }
+
+/**
+ * psc_module_keep_in_reset_enabled() - Keep module in enabled,in-reset state
+ * @mod_num:	LPSC module number
+ * @gate_clocks: Can the clocks be gated on this module?
+ *
+ * Enable the module, but do not release the module from local reset. This is
+ * necessary for many processor systems on keystone SoCs to allow for system
+ * initialization from a master processor prior to releasing the processor
+ * from reset.
+ */
+int psc_module_keep_in_reset_enabled(u32 mod_num, bool gate_clocks)
+{
+	u32 mdctl, ptcmd, mdstat;
+	u32 next_state;
+	int domain_num = psc_get_domain_num(mod_num);
+	int timeout = 100000;
+
+	/* Wait for any previous transitions to complete */
+	psc_wait(domain_num);
+	mdctl = __raw_readl(KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+	/* Should be set 0 to assert Local reset */
+	if ((mdctl & PSC_REG_MDCTL_SET_LRSTZ(mdctl, 1))) {
+		mdctl = PSC_REG_MDCTL_SET_LRSTZ(mdctl, 0);
+		__raw_writel(mdctl, KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+		/* Wait for transition to take place */
+		psc_wait(domain_num);
+	}
+
+	/* Clear Module reset */
+	mdctl = __raw_readl(KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+	next_state = gate_clocks ? PSC_REG_VAL_MDCTL_NEXT_OFF :
+			PSC_REG_VAL_MDCTL_NEXT_ON;
+	mdctl = PSC_REG_MDCTL_SET_NEXT(mdctl, next_state);
+	__raw_writel(mdctl, KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+	/* Trigger PD transition */
+	ptcmd = __raw_readl(KS2_PSC_BASE + PSC_REG_PTCMD);
+	ptcmd |= (u32)(1 << domain_num);
+	__raw_writel(ptcmd, KS2_PSC_BASE + PSC_REG_PTCMD);
+	psc_wait(domain_num);
+
+	mdstat = __raw_readl(KS2_PSC_BASE + PSC_REG_MDSTAT(mod_num));
+	while (timeout) {
+		mdstat = __raw_readl(KS2_PSC_BASE + PSC_REG_MDSTAT(mod_num));
+
+		if (!(PSC_REG_MDSTAT_GET_STATUS(mdstat) & 0x30) &&
+		    PSC_REG_MDSTAT_GET_MRSTDONE(mdstat) &&
+		    PSC_REG_MDSTAT_GET_LRSTDONE(mdstat))
+			break;
+		timeout--;
+	}
+
+	if (!timeout) {
+		printf("%s: Timedout waiting for mdstat(0x%08x) to change\n",
+		       __func__, mdstat);
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+/**
+ * psc_module_release_from_reset() - Release the module from reset
+ * @mod_num:	LPSC module number
+ *
+ * This is the follow through for the command 'psc_module_keep_in_reset_enabled'
+ * Allowing the module to be released from reset once all required inits are
+ * complete for the module. Typically, this allows the processor module to start
+ * execution.
+ */
+int psc_module_release_from_reset(u32 mod_num)
+{
+	u32 mdctl, mdstat;
+	int domain_num = psc_get_domain_num(mod_num);
+	int timeout = 100000;
+
+	/* Wait for any previous transitions to complete */
+	psc_wait(domain_num);
+	mdctl = __raw_readl(KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+	/* Should be set to 1 to de-assert Local reset */
+	if ((mdctl & PSC_REG_MDCTL_SET_LRSTZ(mdctl, 0))) {
+		mdctl = PSC_REG_MDCTL_SET_LRSTZ(mdctl, 1);
+		__raw_writel(mdctl, KS2_PSC_BASE + PSC_REG_MDCTL(mod_num));
+		/* Wait for transition to take place */
+		psc_wait(domain_num);
+	}
+	mdstat = __raw_readl(KS2_PSC_BASE + PSC_REG_MDSTAT(mod_num));
+	while (timeout) {
+		mdstat = __raw_readl(KS2_PSC_BASE + PSC_REG_MDSTAT(mod_num));
+
+		if (!(PSC_REG_MDSTAT_GET_STATUS(mdstat) & 0x30) &&
+		    PSC_REG_MDSTAT_GET_MRSTDONE(mdstat) &&
+		    PSC_REG_MDSTAT_GET_LRSTDONE(mdstat))
+			break;
+		timeout--;
+	}
+
+	if (!timeout) {
+		printf("%s: Timedout waiting for mdstat(0x%08x) to change\n",
+		       __func__, mdstat);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}