[PATCH 3/3] arch: imx8m: Add USB power control

[Angus Ainslie]

Enable turning on USB power for SDP mode

Signed-off-by: Angus Ainslie <angus at akkea.ca>
---
 arch/arm/include/asm/arch-imx8m/sys_proto.h |  1 +
 arch/arm/mach-imx/imx8m/soc.c               | 57 +++++++++++++++++++++
 2 files changed, 58 insertions(+)

diff --git a/arch/arm/include/asm/arch-imx8m/sys_proto.h b/arch/arm/include/asm/arch-imx8m/sys_proto.h
index d328542ece..98a958166a 100644
--- a/arch/arm/include/asm/arch-imx8m/sys_proto.h
+++ b/arch/arm/include/asm/arch-imx8m/sys_proto.h
@@ -14,4 +14,5 @@ void restore_boot_params(void);
 extern unsigned long rom_pointer[];
 enum boot_device get_boot_device(void);
 bool is_usb_boot(void);
+int imx8m_usb_power(int usb_id, bool on);
 #endif
diff --git a/arch/arm/mach-imx/imx8m/soc.c b/arch/arm/mach-imx/imx8m/soc.c
index 863508776d..a43075e1c6 100644
--- a/arch/arm/mach-imx/imx8m/soc.c
+++ b/arch/arm/mach-imx/imx8m/soc.c
@@ -1203,6 +1203,63 @@ int arch_misc_init(void)
 }
 #endif
 
+#ifdef CONFIG_SPL_BUILD
+static u32 gpc_pu_m_core_offset[11] = {
+	0xc00, 0xc40, 0xc80, 0xcc0,
+	0xdc0, 0xe00, 0xe40, 0xe80,
+	0xec0, 0xf00, 0xf40,
+};
+
+#define PGC_PCR				0
+
+void imx_gpc_set_m_core_pgc(unsigned int offset, bool pdn)
+{
+	u32 val;
+	uintptr_t reg = GPC_BASE_ADDR + offset;
+
+	val = readl(reg);
+	val &= ~(0x1 << PGC_PCR);
+
+	if (pdn)
+		val |= 0x1 << PGC_PCR;
+	writel(val, reg);
+}
+
+void imx8m_usb_power_domain(u32 domain_id, bool on)
+{
+	u32 val;
+	uintptr_t reg;
+
+	imx_gpc_set_m_core_pgc(gpc_pu_m_core_offset[domain_id], true);
+
+	reg = GPC_BASE_ADDR + (on ? 0xf8 : 0x104);
+	val = 1 << (domain_id > 3 ? (domain_id + 3) : domain_id);
+	writel(val, reg);
+	while (readl(reg) & val)
+		;
+	imx_gpc_set_m_core_pgc(gpc_pu_m_core_offset[domain_id], false);
+}
+#endif
+
+int imx8m_usb_power(int usb_id, bool on)
+{
+	struct arm_smccc_res res;
+
+	if (usb_id > 1)
+		return -EINVAL;
+
+#ifdef CONFIG_SPL_BUILD
+	imx8m_usb_power_domain(2 + usb_id, on);
+#else
+	arm_smccc_smc(IMX_SIP_GPC, IMX_SIP_GPC_PM_DOMAIN,
+		      2 + usb_id, on, 0, 0, 0, 0, &res);
+	if (res.a0)
+		return -EPERM;
+#endif
+
+	return 0;
+}
+
 void imx_tmu_arch_init(void *reg_base)
 {
 	if (is_imx8mm() || is_imx8mn()) {
-- 
2.25.1