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

[Angus Ainslie]

Enable turning on USB power for SDP mode

Signed-off-by: Angus Ainslie <an...@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