From dd43a038b9eb553ffb864ddef703d41355272bae Mon Sep 17 00:00:00 2001
From: Peter Chen <peter.chen@nxp.com>
Date: Wed, 9 Jan 2019 17:50:10 +0800
Subject: [PATCH 1/1] usb: cdns3: improve drd flow

Signed-off-by: Peter Chen <peter.chen@nxp.com>
---
 drivers/usb/cdns3/Kconfig             |  10 +
 drivers/usb/cdns3/Makefile            |  14 +-
 drivers/usb/cdns3/cdns3-imx.c         | 600 ++++++++++++++++++++++++++++++++++
 drivers/usb/cdns3/cdns3-nxp-reg-def.h | 174 ++++++++++
 drivers/usb/cdns3/core.c              | 272 ++-------------
 drivers/usb/cdns3/core.h              |  35 +-
 drivers/usb/cdns3/drd.c               | 480 ++++++++++++++++-----------
 drivers/usb/cdns3/drd.h               |  14 +-
 drivers/usb/cdns3/gadget.c            |  70 +++-
 drivers/usb/cdns3/gadget.h            |   5 +-
 drivers/usb/cdns3/trace.h             |   4 +-
 11 files changed, 1210 insertions(+), 468 deletions(-)
 create mode 100644 drivers/usb/cdns3/cdns3-imx.c
 create mode 100644 drivers/usb/cdns3/cdns3-nxp-reg-def.h

diff --git a/drivers/usb/cdns3/Kconfig b/drivers/usb/cdns3/Kconfig
index 4adfd87811e8..0e89df18f180 100644
--- a/drivers/usb/cdns3/Kconfig
+++ b/drivers/usb/cdns3/Kconfig
@@ -41,4 +41,14 @@ config USB_CDNS3_PCI_WRAP
 	  If you choose to build this driver as module it will
 	  be dynamically linked and module will be called cdns3-pci.ko
 
+config USB_CDNS3_IMX_WRAP
+	tristate "Cadence USB3 support on NXP i.MX platforms"
+	depends on OF
+	default USB_CDNS3
+	help
+	  If you're using the USBSS Core IP with i.MX platform, please say
+	  'Y' or 'M' here.
+
+	  If you choose to build this driver as module it will
+	  be dynamically linked and module will be called cdns3-imx.ko
 endif
diff --git a/drivers/usb/cdns3/Makefile b/drivers/usb/cdns3/Makefile
index 3f63baa24294..7c2118b6b692 100644
--- a/drivers/usb/cdns3/Makefile
+++ b/drivers/usb/cdns3/Makefile
@@ -2,15 +2,13 @@
 # define_trace.h needs to know how to find our header
 CFLAGS_trace.o				:= -I$(src)
 
-obj-$(CONFIG_USB_CDNS3)			+= cdns3.o
-obj-$(CONFIG_USB_CDNS3_PCI_WRAP)	+= cdns3-pci.o
-
 cdns3-y					:= core.o drd.o trace.o
-
-ifneq ($(CONFIG_DEBUG_FS),)
-	cdns3-y				+= debugfs.o
-endif
+obj-$(CONFIG_USB_CDNS3)			+= cdns3.o
+#ifneq ($(CONFIG_DEBUG_FS),)
+#	cdns3-y				+= debugfs.o
+#endif
 
 cdns3-$(CONFIG_USB_CDNS3_GADGET)	+= gadget.o ep0.o
 cdns3-$(CONFIG_USB_CDNS3_HOST)		+= host.o
-cdns3-pci-y		 		:= cdns3-pci-wrap.o
+obj-$(CONFIG_USB_CDNS3_PCI_WRAP)	+= cdns3-pci-wrap.o
+obj-$(CONFIG_USB_CDNS3_IMX_WRAP)	+= cdns3-imx.o
diff --git a/drivers/usb/cdns3/cdns3-imx.c b/drivers/usb/cdns3/cdns3-imx.c
new file mode 100644
index 000000000000..600fab75b63e
--- /dev/null
+++ b/drivers/usb/cdns3/cdns3-imx.c
@@ -0,0 +1,600 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * cdns3-imx.c - imx Glue layer
+ *
+ * Copyright (C) 2017-2018 NXP
+ *
+ * Authors: Peter Chen <peter.chen@nxp.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/usb/of.h>
+#include <linux/usb/phy.h>
+#include <linux/extcon.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset-controller.h>
+
+#include "cdns3-nxp-reg-def.h"
+#include "core.h"
+
+#define USB3_CORE_CTRL1    0x00
+#define USB3_CORE_CTRL2    0x04
+#define USB3_INT_REG       0x08
+#define USB3_CORE_STATUS   0x0c
+#define XHCI_DEBUG_LINK_ST 0x10
+#define XHCI_DEBUG_BUS     0x14
+#define USB3_SSPHY_CTRL1   0x40
+#define USB3_SSPHY_CTRL2   0x44
+#define USB3_SSPHY_STATUS  0x4c
+#define USB2_PHY_CTRL1     0x50
+#define USB2_PHY_CTRL2     0x54
+#define USB2_PHY_STATUS    0x5c
+
+/* Register bits definition */
+
+/* USB3_CORE_CTRL1 */
+#define SW_RESET_MASK	(0x3f << 26)
+#define PWR_SW_RESET	(1 << 31)
+#define APB_SW_RESET	(1 << 30)
+#define AXI_SW_RESET	(1 << 29)
+#define RW_SW_RESET	(1 << 28)
+#define PHY_SW_RESET	(1 << 27)
+#define PHYAHB_SW_RESET	(1 << 26)
+#define ALL_SW_RESET	(PWR_SW_RESET | APB_SW_RESET | AXI_SW_RESET | \
+		RW_SW_RESET | PHY_SW_RESET | PHYAHB_SW_RESET)
+#define OC_DISABLE	(1 << 9)
+#define MDCTRL_CLK_SEL	(1 << 7)
+#define MODE_STRAP_MASK	(0x7)
+#define DEV_MODE	(1 << 2)
+#define HOST_MODE	(1 << 1)
+#define OTG_MODE	(1 << 0)
+
+/* USB3_INT_REG */
+#define CLK_125_REQ	(1 << 29)
+#define LPM_CLK_REQ	(1 << 28)
+#define DEVU3_WAEKUP_EN	(1 << 14)
+#define OTG_WAKEUP_EN	(1 << 12)
+#define DEV_INT_EN (3 << 8) /* DEV INT b9:8 */
+#define HOST_INT1_EN (1 << 0) /* HOST INT b7:0 */
+
+/* USB3_CORE_STATUS */
+#define MDCTRL_CLK_STATUS	(1 << 15)
+#define DEV_POWER_ON_READY	(1 << 13)
+#define HOST_POWER_ON_READY	(1 << 12)
+
+/* USB3_SSPHY_STATUS */
+#define PHY_REFCLK_REQ		(1 << 0)
+
+#define CDNS3_IMX_CLKS_NUM 5
+static int cdns3_imx_set_mode(struct cdns3 *cdns);
+
+struct cdns3_imx {
+	void __iomem *regs;
+	void __iomem *phy_regs;
+	enum usb_dr_mode role;
+	struct device *dev;
+	struct clk *imx_clks[CDNS3_IMX_CLKS_NUM];
+	struct reset_controller_dev rcdev;
+};
+
+struct cdns3_platform_data cdns3_imx_pdata = {
+	.set_mode = cdns3_imx_set_mode,
+};
+
+#if 1
+static void cdns3_usb_phy_init(void __iomem *regs)
+{
+	u32 value;
+
+	pr_debug("begin of %s\n", __func__);
+
+	writel(0x0830, regs + PHY_PMA_CMN_CTRL1);
+	writel(0x10, regs + TB_ADDR_CMN_DIAG_HSCLK_SEL);
+	writel(0x00F0, regs + TB_ADDR_CMN_PLL0_VCOCAL_INIT_TMR);
+	writel(0x0018, regs + TB_ADDR_CMN_PLL0_VCOCAL_ITER_TMR);
+	writel(0x00D0, regs + TB_ADDR_CMN_PLL0_INTDIV);
+	writel(0x4aaa, regs + TB_ADDR_CMN_PLL0_FRACDIV);
+	writel(0x0034, regs + TB_ADDR_CMN_PLL0_HIGH_THR);
+	writel(0x1ee, regs + TB_ADDR_CMN_PLL0_SS_CTRL1);
+	writel(0x7F03, regs + TB_ADDR_CMN_PLL0_SS_CTRL2);
+	writel(0x0020, regs + TB_ADDR_CMN_PLL0_DSM_DIAG);
+	writel(0x0000, regs + TB_ADDR_CMN_DIAG_PLL0_OVRD);
+	writel(0x0000, regs + TB_ADDR_CMN_DIAG_PLL0_FBH_OVRD);
+	writel(0x0000, regs + TB_ADDR_CMN_DIAG_PLL0_FBL_OVRD);
+	writel(0x0007, regs + TB_ADDR_CMN_DIAG_PLL0_V2I_TUNE);
+	writel(0x0027, regs + TB_ADDR_CMN_DIAG_PLL0_CP_TUNE);
+	writel(0x0008, regs + TB_ADDR_CMN_DIAG_PLL0_LF_PROG);
+	writel(0x0022, regs + TB_ADDR_CMN_DIAG_PLL0_TEST_MODE);
+	writel(0x000a, regs + TB_ADDR_CMN_PSM_CLK_CTRL);
+	writel(0x139, regs + TB_ADDR_XCVR_DIAG_RX_LANE_CAL_RST_TMR);
+	writel(0xbefc, regs + TB_ADDR_XCVR_PSM_RCTRL);
+
+	writel(0x7799, regs + TB_ADDR_TX_PSC_A0);
+	writel(0x7798, regs + TB_ADDR_TX_PSC_A1);
+	writel(0x509b, regs + TB_ADDR_TX_PSC_A2);
+	writel(0x3, regs + TB_ADDR_TX_DIAG_ECTRL_OVRD);
+	writel(0x509b, regs + TB_ADDR_TX_PSC_A3);
+	writel(0x2090, regs + TB_ADDR_TX_PSC_CAL);
+	writel(0x2090, regs + TB_ADDR_TX_PSC_RDY);
+
+	writel(0xA6FD, regs + TB_ADDR_RX_PSC_A0);
+	writel(0xA6FD, regs + TB_ADDR_RX_PSC_A1);
+	writel(0xA410, regs + TB_ADDR_RX_PSC_A2);
+	writel(0x2410, regs + TB_ADDR_RX_PSC_A3);
+
+	writel(0x23FF, regs + TB_ADDR_RX_PSC_CAL);
+	writel(0x2010, regs + TB_ADDR_RX_PSC_RDY);
+
+	writel(0x0020, regs + TB_ADDR_TX_TXCC_MGNLS_MULT_000);
+	writel(0x00ff, regs + TB_ADDR_TX_DIAG_BGREF_PREDRV_DELAY);
+	writel(0x0002, regs + TB_ADDR_RX_SLC_CU_ITER_TMR);
+	writel(0x0013, regs + TB_ADDR_RX_SIGDET_HL_FILT_TMR);
+	writel(0x0000, regs + TB_ADDR_RX_SAMP_DAC_CTRL);
+	writel(0x1004, regs + TB_ADDR_RX_DIAG_SIGDET_TUNE);
+	writel(0x4041, regs + TB_ADDR_RX_DIAG_LFPSDET_TUNE2);
+	writel(0x0480, regs + TB_ADDR_RX_DIAG_BS_TM);
+	writel(0x8006, regs + TB_ADDR_RX_DIAG_DFE_CTRL1);
+	writel(0x003f, regs + TB_ADDR_RX_DIAG_ILL_IQE_TRIM4);
+	writel(0x543f, regs + TB_ADDR_RX_DIAG_ILL_E_TRIM0);
+	writel(0x543f, regs + TB_ADDR_RX_DIAG_ILL_IQ_TRIM0);
+	writel(0x0000, regs + TB_ADDR_RX_DIAG_ILL_IQE_TRIM6);
+	writel(0x8000, regs + TB_ADDR_RX_DIAG_RXFE_TM3);
+	writel(0x0003, regs + TB_ADDR_RX_DIAG_RXFE_TM4);
+	writel(0x2408, regs + TB_ADDR_RX_DIAG_LFPSDET_TUNE);
+	writel(0x05ca, regs + TB_ADDR_RX_DIAG_DFE_CTRL3);
+	writel(0x0258, regs + TB_ADDR_RX_DIAG_SC2C_DELAY);
+	writel(0x1fff, regs + TB_ADDR_RX_REE_VGA_GAIN_NODFE);
+
+	writel(0x02c6, regs + TB_ADDR_XCVR_PSM_CAL_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A0BYP_TMR);
+	writel(0x02c6, regs + TB_ADDR_XCVR_PSM_A0IN_TMR);
+	writel(0x0010, regs + TB_ADDR_XCVR_PSM_A1IN_TMR);
+	writel(0x0010, regs + TB_ADDR_XCVR_PSM_A2IN_TMR);
+	writel(0x0010, regs + TB_ADDR_XCVR_PSM_A3IN_TMR);
+	writel(0x0010, regs + TB_ADDR_XCVR_PSM_A4IN_TMR);
+	writel(0x0010, regs + TB_ADDR_XCVR_PSM_A5IN_TMR);
+
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A0OUT_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A1OUT_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A2OUT_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A3OUT_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A4OUT_TMR);
+	writel(0x0002, regs + TB_ADDR_XCVR_PSM_A5OUT_TMR);
+
+	/* Change rx detect parameter */
+	writel(0x960, regs + TB_ADDR_TX_RCVDET_EN_TMR);
+	writel(0x01e0, regs + TB_ADDR_TX_RCVDET_ST_TMR);
+	writel(0x0090, regs + TB_ADDR_XCVR_DIAG_LANE_FCM_EN_MGN_TMR);
+
+	/* RXDET_IN_P3_32KHZ, Receiver detect slow clock enable */
+	value = readl(regs + TB_ADDR_TX_RCVDETSC_CTRL);
+	value |= RXDET_IN_P3_32KHZ;
+	writel(value, regs + TB_ADDR_TX_RCVDETSC_CTRL);
+
+	udelay(10);
+
+	pr_debug("end of %s\n", __func__);
+}
+#endif
+static void cdns_imx_plat_init(struct cdns3_imx *imx)
+{
+	u32 value;
+	int timeout_us = 100000;
+
+	/* Wait clk value */
+	value = readl(imx->regs + USB3_SSPHY_STATUS);
+	writel(value, imx->regs + USB3_SSPHY_STATUS);
+	udelay(1);
+	value = readl(imx->regs + USB3_SSPHY_STATUS);
+	while ((value & 0xf0000000) != 0xf0000000 && timeout_us-- > 0) {
+		value = readl(imx->regs + USB3_SSPHY_STATUS);
+		dev_dbg(imx->dev, "clkvld:0x%x\n", value);
+		udelay(1);
+	}
+
+	if (timeout_us <= 0)
+		dev_err(imx->dev, "wait clkvld timeout\n");
+
+	/* Set all Reset bits */
+	value = readl(imx->regs + USB3_CORE_CTRL1);
+	value |= ALL_SW_RESET;
+	writel(value, imx->regs + USB3_CORE_CTRL1);
+	udelay(1);
+
+	value = readl(imx->regs + USB3_CORE_CTRL1);
+	value &= ~PHYAHB_SW_RESET;
+	writel(value, imx->regs + USB3_CORE_CTRL1);
+
+	cdns3_usb_phy_init(imx->phy_regs);
+
+	value = readl(imx->regs + USB3_CORE_CTRL1);
+	value &= ~ALL_SW_RESET;
+	writel(value, imx->regs + USB3_CORE_CTRL1);
+
+#if 0
+	if (role == USB_DR_MODE_HOST) {
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+		value = (value & ~MODE_STRAP_MASK) | HOST_MODE | OC_DISABLE;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+		value &= ~PHYAHB_SW_RESET;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+		mdelay(1);
+		/* TODO check the sequence */
+		cdns3_usb_phy_init(imx->phy_regs);
+		/* Force B Session Valid as 1 */
+		writel(0x0060, imx->phy_regs + 0x380a4);
+		mdelay(1);
+
+		value = readl(imx->regs + USB3_INT_REG);
+		value |= HOST_INT1_EN;
+		writel(value, imx->regs + USB3_INT_REG);
+
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+		value &= ~ALL_SW_RESET;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+
+		dev_dbg(imx->dev, "wait xhci_power_on_ready\n");
+
+		value = readl(imx->regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & HOST_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(imx->regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(imx->dev, "wait xhci_power_on_ready timeout\n");
+
+		/*
+		value = readl(xhci_regs + XECP_PORT_CAP_REG);
+		value |= LPM_2_STB_SWITCH_EN;
+		writel(value, xhci_regs + XECP_PORT_CAP_REG);
+		*/
+
+		mdelay(1);
+
+		dev_dbg(imx->dev, "switch to host role successfully\n");
+	} else if (role == USB_DR_MODE_PERIPHERAL) {
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+	//	value = (value & ~MODE_STRAP_MASK) | DEV_MODE;
+	//	writel(value, imx->regs + USB3_CORE_CTRL1);
+		value &= ~PHYAHB_SW_RESET;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+
+		cdns3_usb_phy_init(imx->phy_regs);
+		/* Force B Session Valid as 1 */
+//		writel(0x0060, imx->phy_regs + 0x380a4);
+		value = readl(imx->regs + USB3_INT_REG);
+		value |= DEV_INT_EN;
+		writel(value, imx->regs + USB3_INT_REG);
+
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+		value &= ~ALL_SW_RESET;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+
+		dev_dbg(imx->dev, "wait gadget_power_on_ready\n");
+
+		value = readl(imx->regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & DEV_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(imx->regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(imx->dev,
+				"wait gadget_power_on_ready timeout\n");
+
+		mdelay(1);
+
+		dev_dbg(imx->dev, "switch to gadget role successfully\n");
+	}
+#endif
+}
+
+static int cdns3_imx_set_mode(struct cdns3 *cdns)
+{
+	struct device *dev = cdns->dev->parent;
+	struct cdns3_imx *imx = dev_get_drvdata(dev);
+	enum cdns3_roles role = cdns->role;
+	u32 value;
+	int timeout_us = 100000;
+
+	switch (role) {
+	case CDNS3_ROLE_GADGET:
+		dev_info(dev, "Set controller to Gadget mode\n");
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+		value = (value & ~MODE_STRAP_MASK) | DEV_MODE;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+
+		value = readl(imx->regs + USB3_INT_REG);
+		value |= DEV_INT_EN;
+		writel(value, imx->regs + USB3_INT_REG);
+
+		dev_dbg(dev, "wait gadget_power_on_ready\n");
+
+		value = readl(imx->regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & DEV_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(imx->regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(dev, "wait gadget_power_on_ready timeout\n");
+		break;
+	case CDNS3_ROLE_HOST:
+		dev_info(dev, "Set controller to Host mode\n");
+		value = readl(imx->regs + USB3_CORE_CTRL1);
+		value = (value & ~MODE_STRAP_MASK) | HOST_MODE | OC_DISABLE;
+		writel(value, imx->regs + USB3_CORE_CTRL1);
+
+		value = readl(imx->regs + USB3_INT_REG);
+		value |= HOST_INT1_EN;
+		writel(value, imx->regs + USB3_INT_REG);
+
+		dev_dbg(dev, "wait xhci_power_on_ready\n");
+
+		value = readl(imx->regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & HOST_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(imx->regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(dev, "wait xhci_power_on_ready timeout\n");
+		break;
+	case CDNS3_ROLE_END:
+		dev_info(dev, "reset controller\n");
+		cdns_imx_plat_init(imx);
+		break;
+	}
+
+	return 0;
+}
+
+static int imx_get_clks(struct device *dev)
+{
+	struct cdns3_imx *imx = dev_get_drvdata(dev);
+	int ret = 0;
+
+	imx->imx_clks[0] = devm_clk_get(dev, "usb3_lpm_clk");
+	if (IS_ERR(imx->imx_clks[0])) {
+		ret = PTR_ERR(imx->imx_clks[0]);
+		dev_err(dev, "Failed to get usb3_lpm_clk, err=%d\n", ret);
+		return ret;
+	}
+
+	imx->imx_clks[1] = devm_clk_get(dev, "usb3_bus_clk");
+	if (IS_ERR(imx->imx_clks[1])) {
+		ret = PTR_ERR(imx->imx_clks[1]);
+		dev_err(dev, "Failed to get usb3_bus_clk, err=%d\n", ret);
+		return ret;
+	}
+
+	imx->imx_clks[2] = devm_clk_get(dev, "usb3_aclk");
+	if (IS_ERR(imx->imx_clks[2])) {
+		ret = PTR_ERR(imx->imx_clks[2]);
+		dev_err(dev, "Failed to get usb3_aclk, err=%d\n", ret);
+		return ret;
+	}
+
+	imx->imx_clks[3] = devm_clk_get(dev, "usb3_ipg_clk");
+	if (IS_ERR(imx->imx_clks[3])) {
+		ret = PTR_ERR(imx->imx_clks[3]);
+		dev_err(dev, "Failed to get usb3_ipg_clk, err=%d\n", ret);
+		return ret;
+	}
+
+	imx->imx_clks[4] = devm_clk_get(dev, "usb3_core_pclk");
+	if (IS_ERR(imx->imx_clks[4])) {
+		ret = PTR_ERR(imx->imx_clks[4]);
+		dev_err(dev, "Failed to get usb3_core_pclk, err=%d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int imx_prepare_enable_clks(struct device *dev)
+{
+	struct cdns3_imx *imx = dev_get_drvdata(dev);
+	int i, j, ret = 0;
+
+	for (i = 0; i < CDNS3_IMX_CLKS_NUM; i++) {
+		ret = clk_prepare_enable(imx->imx_clks[i]);
+		if (ret) {
+			dev_err(dev,
+				"Failed to prepare/enable cdns3 clk, err=%d\n",
+				ret);
+			goto err;
+		}
+	}
+
+	return ret;
+err:
+	for (j = i; j > 0; j--)
+		clk_disable_unprepare(imx->imx_clks[j - 1]);
+
+	return ret;
+}
+
+static void imx_disable_unprepare_clks(struct device *dev)
+{
+	struct cdns3_imx *imx = dev_get_drvdata(dev);
+	int i;
+
+	for (i = CDNS3_IMX_CLKS_NUM - 1; i >= 0; i--)
+		clk_disable_unprepare(imx->imx_clks[i]);
+}
+
+static int cdns3_imx_assert(struct reset_controller_dev *r, unsigned long id)
+{
+	struct cdns3_imx *imx = container_of(r, struct cdns3_imx, rcdev);
+	u32 value;
+
+	value = readl(imx->regs + USB3_CORE_CTRL1);
+	if (id == 0)
+		value &= ~PHY_SW_RESET;
+	else if (id == 1)
+		value &= ~PHYAHB_SW_RESET;
+
+	writel(value, imx->regs + USB3_CORE_CTRL1);
+	return 0;
+}
+
+static int cdns3_imx_deassert(struct reset_controller_dev *r, unsigned long id)
+{
+	struct cdns3_imx *imx = container_of(r, struct cdns3_imx, rcdev);
+	u32 value;
+
+	value = readl(imx->regs + USB3_CORE_CTRL1);
+	if (id == 0)
+		value &= ~PHY_SW_RESET;
+	else if (id == 1)
+		value &= ~PHYAHB_SW_RESET;
+
+	writel(value, imx->regs + USB3_CORE_CTRL1);
+	return 0;
+}
+
+static const struct reset_control_ops cdns3_imx_reset_ops = {
+	.assert = cdns3_imx_assert,
+	.deassert = cdns3_imx_deassert,
+};
+
+/**
+ * cdns3_nxp_probe - probe for imx glue driver
+ * @pdev: Pointer to imx glue platform device
+ *
+ * Returns 0 on success otherwise negative errno
+ */
+static int cdns3_imx_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct resource	*res;
+	struct cdns3_imx *imx;
+	void __iomem *regs;
+	int ret;
+	struct of_dev_auxdata imx_auxdata = {
+		.compatible = "cdns,usb3-1.0.0",
+		.platform_data = &cdns3_imx_pdata,
+	};
+
+	dev_dbg(dev, "%s start, pdata=0x%p\n", __func__, &cdns3_imx_pdata);
+	if (!node) {
+		dev_err(dev, "device node not found\n");
+		return -EINVAL;
+	}
+
+	imx = devm_kzalloc(dev, sizeof(*imx), GFP_KERNEL);
+	if (!imx)
+		return -ENOMEM;
+
+	imx->dev = dev;
+	platform_set_drvdata(pdev, imx);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(regs))
+		return PTR_ERR(regs);
+
+	imx->regs = regs;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(regs))
+		return PTR_ERR(regs);
+
+	imx->phy_regs = regs;
+
+	ret = imx_get_clks(dev);
+	if (ret)
+		return ret;
+
+	ret = imx_prepare_enable_clks(dev);
+	if (ret)
+		return ret;
+
+//	imx->role = usb_get_dr_mode(dev);
+//	dev_dbg(dev, "the init role is %d\n", imx->role);
+	cdns_imx_plat_init(imx);
+
+	imx->rcdev.owner = THIS_MODULE;
+	imx->rcdev.ops = &cdns3_imx_reset_ops;
+	imx->rcdev.of_node = dev->of_node;
+	imx->rcdev.nr_resets = 2;
+	ret = reset_controller_register(&imx->rcdev);
+	if (ret)
+		goto err1;
+
+	ret = of_platform_populate(node, NULL, &imx_auxdata, dev);
+	if (ret) {
+		dev_err(dev, "failed to create cnds3 core\n");
+		goto err2;
+	}
+
+	dev_dbg(dev, "Cadence USB3 core: probe succeed\n");
+
+	return 0;
+err2:
+	reset_controller_unregister(&imx->rcdev);
+err1:
+	imx_disable_unprepare_clks(dev);
+	return ret;
+}
+
+/**
+ * cdns3_imx_remove - remove glue imx driver
+ * @pdev: Pointer to Linux platform device
+ *
+ * Returns 0 on success otherwise negative errno
+ */
+static int cdns3_imx_remove(struct platform_device *pdev)
+{
+	imx_disable_unprepare_clks(&pdev->dev);
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id of_cdns3_imx_match[] = {
+	{ .compatible = "nxp,cdns3" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, of_cdns3_match);
+#endif
+
+static struct platform_driver cdns3_imx_driver = {
+	.probe		= cdns3_imx_probe,
+	.remove		= cdns3_imx_remove,
+	.driver		= {
+		.name	= "nxp-cdns3",
+		.of_match_table	= of_match_ptr(of_cdns3_imx_match),
+	},
+};
+
+static int __init cdns3_imx_register(void)
+{
+	return platform_driver_register(&cdns3_imx_driver);
+}
+module_init(cdns3_imx_register);
+
+static void __exit cdns3_imx_unregister(void)
+{
+	platform_driver_unregister(&cdns3_imx_driver);
+}
+module_exit(cdns3_imx_unregister);
+
+MODULE_ALIAS("platform:cdns3-nxp");
+MODULE_AUTHOR("Peter Chen <peter.chen@nxp.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Cadence USB3 imx glue driver");
diff --git a/drivers/usb/cdns3/cdns3-nxp-reg-def.h b/drivers/usb/cdns3/cdns3-nxp-reg-def.h
new file mode 100644
index 000000000000..4ac73005c023
--- /dev/null
+++ b/drivers/usb/cdns3/cdns3-nxp-reg-def.h
@@ -0,0 +1,174 @@
+/**
+ * cdns3-nxp-reg-def.h - nxp wrap layer register definition
+ *
+ * Copyright 2017 NXP
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DRIVERS_USB_CDNS3_NXP_H
+#define __DRIVERS_USB_CDNS3_NXP_H
+
+#define USB3_CORE_CTRL1    0x00
+#define USB3_CORE_CTRL2    0x04
+#define USB3_INT_REG       0x08
+#define USB3_CORE_STATUS   0x0c
+#define XHCI_DEBUG_LINK_ST 0x10
+#define XHCI_DEBUG_BUS     0x14
+#define USB3_SSPHY_CTRL1   0x40
+#define USB3_SSPHY_CTRL2   0x44
+#define USB3_SSPHY_STATUS  0x4c
+#define USB2_PHY_CTRL1     0x50
+#define USB2_PHY_CTRL2     0x54
+#define USB2_PHY_STATUS    0x5c
+
+/* Register bits definition */
+
+/* USB3_CORE_CTRL1 */
+#define SW_RESET_MASK	(0x3f << 26)
+#define PWR_SW_RESET	(1 << 31)
+#define APB_SW_RESET	(1 << 30)
+#define AXI_SW_RESET	(1 << 29)
+#define RW_SW_RESET	(1 << 28)
+#define PHY_SW_RESET	(1 << 27)
+#define PHYAHB_SW_RESET	(1 << 26)
+#define ALL_SW_RESET	(PWR_SW_RESET | APB_SW_RESET | AXI_SW_RESET | \
+		RW_SW_RESET | PHY_SW_RESET | PHYAHB_SW_RESET)
+#define OC_DISABLE	(1 << 9)
+#define MDCTRL_CLK_SEL	(1 << 7)
+#define MODE_STRAP_MASK	(0x7)
+#define DEV_MODE	(1 << 2)
+#define HOST_MODE	(1 << 1)
+#define OTG_MODE	(1 << 0)
+
+/* USB3_INT_REG */
+#define CLK_125_REQ	(1 << 29)
+#define LPM_CLK_REQ	(1 << 28)
+#define DEVU3_WAEKUP_EN	(1 << 14)
+#define OTG_WAKEUP_EN	(1 << 12)
+#define DEV_INT_EN (3 << 8) /* DEV INT b9:8 */
+#define HOST_INT1_EN (1 << 0) /* HOST INT b7:0 */
+
+/* USB3_CORE_STATUS */
+#define MDCTRL_CLK_STATUS	(1 << 15)
+#define DEV_POWER_ON_READY	(1 << 13)
+#define HOST_POWER_ON_READY	(1 << 12)
+
+/* USB3_SSPHY_STATUS */
+#define PHY_REFCLK_REQ		(1 << 0)
+
+
+/* PHY register definition */
+#define PHY_PMA_CMN_CTRL1			(0xC800 * 4)
+#define TB_ADDR_CMN_DIAG_HSCLK_SEL		(0x01e0 * 4)
+#define TB_ADDR_CMN_PLL0_VCOCAL_INIT_TMR	(0x0084 * 4)
+#define TB_ADDR_CMN_PLL0_VCOCAL_ITER_TMR	(0x0085 * 4)
+#define TB_ADDR_CMN_PLL0_INTDIV	                (0x0094 * 4)
+#define TB_ADDR_CMN_PLL0_FRACDIV		(0x0095 * 4)
+#define TB_ADDR_CMN_PLL0_HIGH_THR		(0x0096 * 4)
+#define TB_ADDR_CMN_PLL0_SS_CTRL1		(0x0098 * 4)
+#define TB_ADDR_CMN_PLL0_SS_CTRL2		(0x0099 * 4)
+#define TB_ADDR_CMN_PLL0_DSM_DIAG		(0x0097 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_OVRD		(0x01c2 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_FBH_OVRD		(0x01c0 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_FBL_OVRD		(0x01c1 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_V2I_TUNE          (0x01C5 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_CP_TUNE           (0x01C6 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_LF_PROG           (0x01C7 * 4)
+#define TB_ADDR_CMN_DIAG_PLL0_TEST_MODE		(0x01c4 * 4)
+#define TB_ADDR_CMN_PSM_CLK_CTRL		(0x0061 * 4)
+#define TB_ADDR_XCVR_DIAG_RX_LANE_CAL_RST_TMR	(0x40ea * 4)
+#define TB_ADDR_XCVR_PSM_RCTRL	                (0x4001 * 4)
+#define TB_ADDR_TX_PSC_A0		        (0x4100 * 4)
+#define TB_ADDR_TX_PSC_A1		        (0x4101 * 4)
+#define TB_ADDR_TX_PSC_A2		        (0x4102 * 4)
+#define TB_ADDR_TX_PSC_A3		        (0x4103 * 4)
+#define TB_ADDR_TX_DIAG_ECTRL_OVRD		(0x41f5 * 4)
+#define TB_ADDR_TX_PSC_CAL		        (0x4106 * 4)
+#define TB_ADDR_TX_PSC_RDY		        (0x4107 * 4)
+#define TB_ADDR_RX_PSC_A0	                (0x8000 * 4)
+#define TB_ADDR_RX_PSC_A1	                (0x8001 * 4)
+#define TB_ADDR_RX_PSC_A2	                (0x8002 * 4)
+#define TB_ADDR_RX_PSC_A3	                (0x8003 * 4)
+#define TB_ADDR_RX_PSC_CAL	                (0x8006 * 4)
+#define TB_ADDR_RX_PSC_RDY	                (0x8007 * 4)
+#define TB_ADDR_TX_TXCC_MGNLS_MULT_000		(0x4058 * 4)
+#define TB_ADDR_TX_DIAG_BGREF_PREDRV_DELAY	(0x41e7 * 4)
+#define TB_ADDR_RX_SLC_CU_ITER_TMR		(0x80e3 * 4)
+#define TB_ADDR_RX_SIGDET_HL_FILT_TMR		(0x8090 * 4)
+#define TB_ADDR_RX_SAMP_DAC_CTRL		(0x8058 * 4)
+#define TB_ADDR_RX_DIAG_SIGDET_TUNE		(0x81dc * 4)
+#define TB_ADDR_RX_DIAG_LFPSDET_TUNE2		(0x81df * 4)
+#define TB_ADDR_RX_DIAG_BS_TM	                (0x81f5 * 4)
+#define TB_ADDR_RX_DIAG_DFE_CTRL1		(0x81d3 * 4)
+#define TB_ADDR_RX_DIAG_ILL_IQE_TRIM4		(0x81c7 * 4)
+#define TB_ADDR_RX_DIAG_ILL_E_TRIM0		(0x81c2 * 4)
+#define TB_ADDR_RX_DIAG_ILL_IQ_TRIM0		(0x81c1 * 4)
+#define TB_ADDR_RX_DIAG_ILL_IQE_TRIM6		(0x81c9 * 4)
+#define TB_ADDR_RX_DIAG_RXFE_TM3		(0x81f8 * 4)
+#define TB_ADDR_RX_DIAG_RXFE_TM4		(0x81f9 * 4)
+#define TB_ADDR_RX_DIAG_LFPSDET_TUNE		(0x81dd * 4)
+#define TB_ADDR_RX_DIAG_DFE_CTRL3		(0x81d5 * 4)
+#define TB_ADDR_RX_DIAG_SC2C_DELAY		(0x81e1 * 4)
+#define TB_ADDR_RX_REE_VGA_GAIN_NODFE		(0x81bf * 4)
+#define TB_ADDR_XCVR_PSM_CAL_TMR		(0x4002 * 4)
+#define TB_ADDR_XCVR_PSM_A0BYP_TMR		(0x4004 * 4)
+#define TB_ADDR_XCVR_PSM_A0IN_TMR		(0x4003 * 4)
+#define TB_ADDR_XCVR_PSM_A1IN_TMR		(0x4005 * 4)
+#define TB_ADDR_XCVR_PSM_A2IN_TMR		(0x4006 * 4)
+#define TB_ADDR_XCVR_PSM_A3IN_TMR		(0x4007 * 4)
+#define TB_ADDR_XCVR_PSM_A4IN_TMR		(0x4008 * 4)
+#define TB_ADDR_XCVR_PSM_A5IN_TMR		(0x4009 * 4)
+#define TB_ADDR_XCVR_PSM_A0OUT_TMR		(0x400a * 4)
+#define TB_ADDR_XCVR_PSM_A1OUT_TMR		(0x400b * 4)
+#define TB_ADDR_XCVR_PSM_A2OUT_TMR		(0x400c * 4)
+#define TB_ADDR_XCVR_PSM_A3OUT_TMR		(0x400d * 4)
+#define TB_ADDR_XCVR_PSM_A4OUT_TMR		(0x400e * 4)
+#define TB_ADDR_XCVR_PSM_A5OUT_TMR		(0x400f * 4)
+#define TB_ADDR_TX_RCVDET_EN_TMR	        (0x4122 * 4)
+#define TB_ADDR_TX_RCVDET_ST_TMR	        (0x4123 * 4)
+#define TB_ADDR_XCVR_DIAG_LANE_FCM_EN_MGN_TMR	(0x40f2 * 4)
+#define TB_ADDR_TX_RCVDETSC_CTRL	        (0x4124 * 4)
+
+/* Register bits definition */
+
+/* TB_ADDR_TX_RCVDETSC_CTRL */
+#define RXDET_IN_P3_32KHZ			(1 << 0)
+
+/* OTG registers definition */
+#define OTGSTS		0x4
+#define OTGREFCLK	0xc
+
+/* Register bits definition */
+/* OTGSTS */
+#define OTG_NRDY	(1 << 11)
+/* OTGREFCLK */
+#define OTG_STB_CLK_SWITCH_EN	(1 << 31)
+
+/* xHCI registers definition  */
+#define XECP_PORT_CAP_REG	0x8000
+#define XECP_PM_PMCSR		0x8018
+#define XECP_AUX_CTRL_REG1	0x8120
+
+/* Register bits definition */
+/* XECP_PORT_CAP_REG */
+#define LPM_2_STB_SWITCH_EN	(1 << 25)
+
+/* XECP_AUX_CTRL_REG1 */
+#define CFG_RXDET_P3_EN		(1 << 15)
+
+/* XECP_PM_PMCSR */
+#define PS_MASK			(3 << 0)
+#define PS_D0			0
+#define PS_D1			(1 << 0)
+#endif /* __DRIVERS_USB_CDNS3_NXP_H */
diff --git a/drivers/usb/cdns3/core.c b/drivers/usb/cdns3/core.c
index d274586aca36..cadd249fbdaf 100644
--- a/drivers/usb/cdns3/core.c
+++ b/drivers/usb/cdns3/core.c
@@ -14,88 +14,13 @@
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
-#include <linux/pm_runtime.h>
 
 #include "gadget.h"
 #include "core.h"
+#include "drd.h"
 #include "host-export.h"
 #include "gadget-export.h"
-#include "drd.h"
-#include "debug.h"
-
-static inline
-struct cdns3_role_driver *cdns3_get_current_role_driver(struct cdns3 *cdns)
-{
-	WARN_ON(cdns->role >= CDNS3_ROLE_END || !cdns->roles[cdns->role]);
-	return cdns->roles[cdns->role];
-}
-
-static inline int cdns3_role_start(struct cdns3 *cdns, enum cdns3_roles role)
-{
-	int ret;
-
-	if (WARN_ON(role >= CDNS3_ROLE_END))
-		return 0;
-
-	if (!cdns->roles[role])
-		return -ENXIO;
-
-	if (cdns->roles[role]->state == CDNS3_ROLE_STATE_ACTIVE)
-		return 0;
-
-	mutex_lock(&cdns->mutex);
-	cdns->role = role;
-	ret = cdns->roles[role]->start(cdns);
-	if (!ret)
-		cdns->roles[role]->state = CDNS3_ROLE_STATE_ACTIVE;
-	mutex_unlock(&cdns->mutex);
-	return ret;
-}
-
-void cdns3_role_stop(struct cdns3 *cdns)
-{
-	enum cdns3_roles role = cdns->role;
-
-	if (role >= CDNS3_ROLE_END) {
-		WARN_ON(role > CDNS3_ROLE_END);
-		return;
-	}
-
-	if (cdns->roles[role]->state == CDNS3_ROLE_STATE_INACTIVE)
-		return;
-
-	mutex_lock(&cdns->mutex);
-	cdns->roles[role]->stop(cdns);
-	cdns->roles[role]->state = CDNS3_ROLE_STATE_INACTIVE;
-	mutex_unlock(&cdns->mutex);
-}
-
-/*
- * cdns->role gets from cdns3_get_initial_role, and this API tells role at the
- * runtime.
- * If both roles are supported, the role is selected based on vbus/id.
- * It could be read from OTG register or external connector.
- * If only single role is supported, only one role structure
- * is allocated, cdns->roles[CDNS3_ROLE_HOST] or cdns->roles[CDNS3_ROLE_GADGET].
- */
-static enum cdns3_roles cdns3_get_initial_role(struct cdns3 *cdns)
-{
-	if (cdns->roles[CDNS3_ROLE_HOST] && cdns->roles[CDNS3_ROLE_GADGET]) {
-		if (cdns3_is_host(cdns))
-			return CDNS3_ROLE_HOST;
-		if (cdns3_is_device(cdns))
-			return CDNS3_ROLE_GADGET;
-	}
-	return cdns->roles[CDNS3_ROLE_HOST]
-		? CDNS3_ROLE_HOST
-		: CDNS3_ROLE_GADGET;
-}
-
-static void cdns3_exit_roles(struct cdns3 *cdns)
-{
-	cdns3_role_stop(cdns);
-	cdns3_drd_exit(cdns);
-}
+//#include "debug.h"
 
 /**
  * cdns3_core_init_role - initialize role of operation
@@ -106,162 +31,37 @@ static void cdns3_exit_roles(struct cdns3 *cdns)
 static int cdns3_core_init_role(struct cdns3 *cdns)
 {
 	struct device *dev = cdns->dev;
-	enum usb_dr_mode best_dr_mode;
-	enum usb_dr_mode dr_mode;
+	enum usb_dr_mode dr_mode = cdns->dr_mode;
 	int ret = 0;
 
-	dr_mode = usb_get_dr_mode(dev);
-	cdns->role = CDNS3_ROLE_END;
-
-	/*
-	 * If driver can't read mode by means of usb_get_dr_mdoe function then
-	 * chooses mode according with Kernel configuration. This setting
-	 * can be restricted later depending on strap pin configuration.
-	 */
-	if (dr_mode == USB_DR_MODE_UNKNOWN) {
-		if (IS_ENABLED(CONFIG_USB_CDNS3_HOST) &&
-		    IS_ENABLED(CONFIG_USB_CDNS3_GADGET))
-			dr_mode = USB_DR_MODE_OTG;
-		else if (IS_ENABLED(CONFIG_USB_CDNS3_HOST))
-			dr_mode = USB_DR_MODE_HOST;
-		else if (IS_ENABLED(CONFIG_USB_CDNS3_GADGET))
-			dr_mode = USB_DR_MODE_PERIPHERAL;
-	}
-
-	best_dr_mode = USB_DR_MODE_OTG;
-
-	if (dr_mode == USB_DR_MODE_OTG) {
-		best_dr_mode = cdns->dr_mode;
-	} else if (cdns->dr_mode == USB_DR_MODE_OTG) {
-		best_dr_mode = dr_mode;
-	} else if (cdns->dr_mode != dr_mode) {
-		dev_err(dev, "Incorrect DRD configuration\n");
-		return -EINVAL;
-	}
-
-	dr_mode = best_dr_mode;
-
 	if (dr_mode == USB_DR_MODE_OTG || dr_mode == USB_DR_MODE_HOST) {
 		ret = cdns3_host_init(cdns);
 		if (ret) {
-			dev_err(dev, "Host initialization failed with %d\n",
-				ret);
-			goto err;
+			if (ret == -ENXIO)
+				dev_dbg(dev, "doesn't support host\n");
+			else
+				return ret;
 		}
 	}
 
 	if (dr_mode == USB_DR_MODE_OTG || dr_mode == USB_DR_MODE_PERIPHERAL) {
 		ret = cdns3_gadget_init(cdns);
 		if (ret) {
-			dev_err(dev, "Device initialization failed with %d\n",
-				ret);
-			goto err;
+			if (ret == -ENXIO)
+				dev_dbg(dev, "doesn't support gadget\n");
+			else
+				return ret;
 		}
 	}
 
-	cdns->desired_dr_mode = dr_mode;
-	cdns->dr_mode = dr_mode;
-	/*
-	 * dr_mode could be change so DRD must update controller
-	 * configuration
-	 */
-	ret = cdns3_drd_update_mode(cdns);
-
-	cdns->role = cdns3_get_initial_role(cdns);
-
-	ret = cdns3_role_start(cdns, cdns->role);
-	if (ret) {
-		dev_err(dev, "can't start %s role\n",
-			cdns3_get_current_role_driver(cdns)->name);
-		goto err;
+	if (!cdns->roles[CDNS3_ROLE_HOST] && !cdns->roles[CDNS3_ROLE_GADGET]) {
+		dev_err(dev, "no supported roles\n");
+		return -ENODEV;
 	}
 
-	return ret;
-err:
-	cdns3_exit_roles(cdns);
 	return ret;
 }
 
-/**
- * cdsn3_get_real_role - get real role of controller based on hardware settings.
- * @cdns: Pointer to cdns3 structure
- *
- * Returns role
- */
-enum cdns3_roles cdsn3_get_real_role(struct cdns3 *cdns)
-{
-	enum cdns3_roles role = CDNS3_ROLE_END;
-
-	if (cdns->current_dr_mode == USB_DR_MODE_OTG) {
-		if (cdns3_get_id(cdns))
-			role = CDNS3_ROLE_GADGET;
-		else
-			role = CDNS3_ROLE_HOST;
-	} else {
-		if (cdns3_is_host(cdns))
-			role = CDNS3_ROLE_HOST;
-		if (cdns3_is_device(cdns))
-			role = CDNS3_ROLE_GADGET;
-	}
-
-	return role;
-}
-
-/**
- * cdns3_role_switch - work queue handler for role switch
- *
- * @work: work queue item structure
- *
- * Handles below events:
- * - Role switch for dual-role devices
- * - CDNS3_ROLE_GADGET <--> CDNS3_ROLE_END for peripheral-only devices
- */
-static void cdns3_role_switch(struct work_struct *work)
-{
-	enum cdns3_roles role = CDNS3_ROLE_END;
-	struct cdns3_role_driver *role_drv;
-	enum cdns3_roles current_role;
-	struct cdns3 *cdns;
-	int ret = 0;
-
-	cdns = container_of(work, struct cdns3, role_switch_wq);
-
-	/* During switching cdns->role can be different then role */
-	role = cdsn3_get_real_role(cdns);
-
-	role_drv = cdns3_get_current_role_driver(cdns);
-
-	pm_runtime_get_sync(cdns->dev);
-
-	/* Disable current role. This state can be forced from user space. */
-	if (cdns->debug_disable && role_drv->state == CDNS3_ROLE_STATE_ACTIVE) {
-		cdns3_role_stop(cdns);
-		goto exit;
-	}
-
-	/* Do nothing if nothing changed */
-	if (cdns->role == role && role_drv->state == CDNS3_ROLE_STATE_ACTIVE)
-		goto exit;
-
-	cdns3_role_stop(cdns);
-
-	role = cdsn3_get_real_role(cdns);
-
-	current_role = cdns->role;
-	dev_dbg(cdns->dev, "Switching role");
-
-	ret = cdns3_role_start(cdns, role);
-
-	if (ret) {
-		/* Back to current role */
-		dev_err(cdns->dev, "set %d has failed, back to %d\n",
-			role, current_role);
-		cdns3_role_start(cdns, current_role);
-	}
-exit:
-	pm_runtime_put_sync(cdns->dev);
-}
-
 /**
  * cdns3_probe - probe for cdns3 core device
  * @pdev: Pointer to cdns3 core platform device
@@ -276,12 +76,13 @@ static int cdns3_probe(struct platform_device *pdev)
 	void __iomem *regs;
 	int ret;
 
+	dev_dbg(dev, "%s, start\n", __func__);
 	cdns = devm_kzalloc(dev, sizeof(*cdns), GFP_KERNEL);
 	if (!cdns)
 		return -ENOMEM;
 
 	cdns->dev = dev;
-
+	cdns->pdata = dev_get_platdata(dev);
 	platform_set_drvdata(pdev, cdns);
 
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
@@ -326,11 +127,11 @@ static int cdns3_probe(struct platform_device *pdev)
 		}
 	}
 
-	phy_init(cdns->phy);
-
-	INIT_WORK(&cdns->role_switch_wq, cdns3_role_switch);
+	ret = phy_init(cdns->phy);
+	if (ret)
+		goto err0;
 
-	ret = cdns3_drd_init(cdns);
+	ret = cdns3_finalize_dr_mode(cdns);
 	if (ret)
 		goto err1;
 
@@ -338,20 +139,21 @@ static int cdns3_probe(struct platform_device *pdev)
 	if (ret)
 		goto err1;
 
-	cdns3_debugfs_init(cdns);
-	device_set_wakeup_capable(dev, true);
-	pm_runtime_set_active(dev);
-	pm_runtime_enable(dev);
+	if (cdns->roles[CDNS3_ROLE_GADGET]) {
+		ret = cdns3_drd_init(cdns);
+		if (ret) {
+			dev_err(dev, "init otg fails, ret = %d\n", ret);
+			goto err1;
+		}
+	}
 
-	/*
-	 * The controller needs less time between bus and controller suspend,
-	 * and we also needs a small delay to avoid frequently entering low
-	 * power mode.
-	 */
-	pm_runtime_set_autosuspend_delay(dev, 20);
-	pm_runtime_mark_last_busy(dev);
-	pm_runtime_use_autosuspend(dev);
-	dev_dbg(dev, "Cadence USB3 core: probe succeed\n");
+	cdns->role = cdns3_get_role(cdns);
+	ret = cdns3_role_start(cdns, cdns->role);
+	if (ret) {
+		dev_err(dev, "can't start %s role\n",
+			cdns3_get_current_role_driver(cdns)->name);
+		goto err1;
+	}
 
 	return 0;
 
@@ -371,11 +173,7 @@ static int cdns3_remove(struct platform_device *pdev)
 {
 	struct cdns3 *cdns = platform_get_drvdata(pdev);
 
-	pm_runtime_get_sync(&pdev->dev);
-	pm_runtime_disable(&pdev->dev);
-	pm_runtime_put_noidle(&pdev->dev);
-	cdns3_debugfs_exit(cdns);
-	cdns3_exit_roles(cdns);
+	cdns3_role_stop(cdns);
 	phy_exit(cdns->phy);
 	return 0;
 }
diff --git a/drivers/usb/cdns3/core.h b/drivers/usb/cdns3/core.h
index fb4b39206158..a91342b1a868 100644
--- a/drivers/usb/cdns3/core.h
+++ b/drivers/usb/cdns3/core.h
@@ -41,6 +41,10 @@ struct cdns3_role_driver {
 	int state;
 };
 
+struct cdns3_platform_data {
+	int (*set_mode) (struct cdns3 *cdns);
+};
+
 #define CDNS3_XHCI_RESOURCES_NUM	2
 /**
  * struct cdns3 - Representation of Cadence USB3 DRD controller.
@@ -56,28 +60,15 @@ struct cdns3_role_driver {
  * @gadget_dev: the child gadget device pointer for cdns3 core
  * @usb: phy for this controller
  * @role_switch_wq: work queue item for role switch
- * @in_lpm: the controller in low power mode
- * @wakeup_int: the wakeup interrupt
  * @mutex: the mutex for concurrent code at driver
  * @dr_mode: supported mode of operation it can be only Host, only Device
  *           or OTG mode that allow to switch between Device and Host mode.
  *           This field based on firmware setting, kernel configuration
  *           and hardware configuration.
- * @current_dr_mode: current mode of operation when in dual-role mode
- * @desired_dr_mode: desired mode of operation when in dual-role mode.
- *           This value can be changed during runtime.
- *           Available options depends on  dr_mode:
- *           dr_mode                 |  desired_dr_mode and current_dr_mode
- *           ----------------------------------------------------------------
- *           USB_DR_MODE_HOST        | only USB_DR_MODE_HOST
- *           USB_DR_MODE_PERIPHERAL  | only USB_DR_MODE_PERIPHERAL
- *           USB_DR_MODE_OTG         | only USB_DR_MODE_HOST
- *           USB_DR_MODE_OTG         | only USB_DR_MODE_PERIPHERAL
- *           USB_DR_MODE_OTG         | USB_DR_MODE_OTG
- *
- *           Desired_dr_role can be changed by means of debugfs.
  * @root: debugfs root folder pointer
  * @debug_disable:
+ * @extcon: Extern connector
+ * @extcon_nb: notifier block for Type-C extern connector
  */
 struct cdns3 {
 	struct device			*dev;
@@ -100,17 +91,21 @@ struct cdns3 {
 	struct cdns3_device		*gadget_dev;
 	struct phy			*phy;
 	struct work_struct		role_switch_wq;
-	int				in_lpm:1;
-	int				wakeup_int:1;
 	/* mutext used in workqueue*/
 	struct mutex			mutex;
 	enum usb_dr_mode		dr_mode;
-	enum usb_dr_mode		current_dr_mode;
-	enum usb_dr_mode		desired_dr_mode;
 	struct dentry			*root;
 	int				debug_disable:1;
+	struct extcon_dev 		*extcon;
+	struct notifier_block 		extcon_nb;
+	struct cdns3_platform_data 	*pdata;
 };
 
-void cdns3_role_stop(struct cdns3 *cdns);
+static inline struct cdns3_role_driver *cdns3_get_current_role_driver(
+	struct cdns3 *cdns)
+{
+	WARN_ON(cdns->role >= CDNS3_ROLE_END || !cdns->roles[cdns->role]);
+	return cdns->roles[cdns->role];
+}
 
 #endif /* __LINUX_CDNS3_CORE_H */
diff --git a/drivers/usb/cdns3/drd.c b/drivers/usb/cdns3/drd.c
index b0c32302eb0b..56d2b4ca15f8 100644
--- a/drivers/usb/cdns3/drd.c
+++ b/drivers/usb/cdns3/drd.c
@@ -4,95 +4,55 @@
  *
  * Copyright (C) 2018 Cadence.
  *
- * Author: Pawel Laszczak <pawell@cadence.com
+ * Author: Pawel Laszczak <pawell@cadence.com>
  *
  */
 #include <linux/kernel.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/usb/otg.h>
+#include <linux/extcon.h>
 
 #include "gadget.h"
-#include "drd.h"
 #include "core.h"
+#include "drd.h"
 
-static int cdns3_drd_switch_gadget(struct cdns3 *cdns, int on);
-static int cdns3_drd_switch_host(struct cdns3 *cdns, int on);
-
-/**
- * cdns3_set_mode - change mode of OTG Core
- * @cdns: pointer to context structure
- * @mode: selected mode from cdns_role
- */
-void cdns3_set_mode(struct cdns3 *cdns, enum usb_dr_mode mode)
+static int cdns3_get_id(struct cdns3 *cdns)
 {
-	u32 reg;
-
-	cdns->current_dr_mode = mode;
-
-	switch (mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		dev_info(cdns->dev, "Set controller to Gadget mode\n");
-		cdns3_drd_switch_gadget(cdns, 1);
-		break;
-	case USB_DR_MODE_HOST:
-		dev_info(cdns->dev, "Set controller to Host mode\n");
-		cdns3_drd_switch_host(cdns, 1);
-		break;
-	case USB_DR_MODE_OTG:
-		dev_info(cdns->dev, "Set controller to OTG mode\n");
-		if (cdns->version == CDNS3_CONTROLLER_V1) {
-			reg = readl(&cdns->otg_v1_regs->override);
-			reg |= OVERRIDE_IDPULLUP;
-			writel(reg, &cdns->otg_v1_regs->override);
-		} else {
-			reg = readl(&cdns->otg_v0_regs->ctrl1);
-			reg |= OVERRIDE_IDPULLUP_V0;
-			writel(reg, &cdns->otg_v0_regs->ctrl1);
-		}
-
-		/*
-		 * Hardware specification says: "ID_VALUE must be valid within
-		 * 50ms after idpullup is set to '1" so driver must wait
-		 * 50ms before reading this pin.
-		 */
-		usleep_range(50000, 60000);
-		break;
-	default:
-		cdns->current_dr_mode = USB_DR_MODE_UNKNOWN;
-		dev_err(cdns->dev, "Unsupported mode of operation %d\n", mode);
-		return;
-	}
+	return !!(readl(&cdns->otg_regs->sts) & OTGSTS_ID_VALUE);
 }
 
-int cdns3_get_id(struct cdns3 *cdns)
+static int cdns3_get_vbus(struct cdns3 *cdns)
 {
-	int id;
-
-	id = readl(&cdns->otg_regs->sts) & OTGSTS_ID_VALUE;
-	dev_dbg(cdns->dev, "OTG ID: %d", id);
-	return  1 ; //id;
+	return !!(readl(&cdns->otg_regs->sts) & OTGSTS_SESSION_VALID);
 }
 
-int cdns3_is_host(struct cdns3 *cdns)
+static int cdns3_is_host(struct cdns3 *cdns)
 {
-	if (cdns->current_dr_mode == USB_DR_MODE_HOST)
-		return 1;
-	else if (!cdns3_get_id(cdns))
+	if (cdns->extcon) {
+		if (extcon_get_state(cdns->extcon, EXTCON_USB_HOST))
+			return 1;
+		else
+			return 0;
+	} else if (!cdns3_get_id(cdns)) {
 		return 1;
-
-	return 0;
+	} else {
+		return 0;
+	}
 }
 
-int cdns3_is_device(struct cdns3 *cdns)
+static int cdns3_is_device(struct cdns3 *cdns)
 {
-	if (cdns->current_dr_mode == USB_DR_MODE_PERIPHERAL)
-		return 1;
-	else if (cdns->current_dr_mode == USB_DR_MODE_OTG)
-		if (cdns3_get_id(cdns))
+	if (cdns->extcon) {
+		if (extcon_get_state(cdns->extcon, EXTCON_USB))
 			return 1;
-
-	return 0;
+		else
+			return 0;
+	} else if (cdns3_get_id(cdns) && cdns3_get_vbus(cdns)) {
+		return 1;
+	} else {
+		return 0;
+	}
 }
 
 /**
@@ -108,136 +68,104 @@ static void cdns3_otg_disable_irq(struct cdns3 *cdns)
  * cdns3_otg_enable_irq - enable id and sess_valid interrupts
  * @cdns: Pointer to controller context structure
  */
-static void cdns3_otg_enable_irq(struct cdns3 *cdns)
+static void cdns3_enable_id_irq(struct cdns3 *cdns)
 {
-	writel(OTGIEN_ID_CHANGE_INT | OTGIEN_VBUSVALID_RISE_INT |
-	       OTGIEN_VBUSVALID_FALL_INT, &cdns->otg_regs->ien);
+	writel(OTGIEN_ID_CHANGE_INT, &cdns->otg_regs->ien);
 }
 
-/**
- * cdns3_drd_switch_host - start/stop host
- * @cdns: Pointer to controller context structure
- * @on: 1 for start, 0 for stop
- *
- * Returns 0 on success otherwise negative errno
- */
-static int cdns3_drd_switch_host(struct cdns3 *cdns, int on)
+static void cdns3_enable_vbus_irq(struct cdns3 *cdns, bool on)
 {
-	int ret;
-	u32 reg = OTGCMD_OTG_DIS;
-
-	/* switch OTG core */
-	if (on) {
-		writel(OTGCMD_HOST_BUS_REQ | reg, &cdns->otg_regs->cmd);
-
-		dev_dbg(cdns->dev, "Waiting for Host mode is turned on\n");
-		ret = cdns3_handshake(&cdns->otg_regs->sts, OTGSTS_XHCI_READY,
-				      OTGSTS_XHCI_READY, 100000);
+	u32 value = readl(&cdns->otg_regs->ien);
+	u32 vbus = (OTGIEN_VBUSSESS_RISE_INT | OTGIEN_VBUSSESS_FALL_INT);
 
-		if (ret)
-			return ret;
-	} else {
-		usleep_range(30, 40);
-		writel(OTGCMD_HOST_BUS_DROP | OTGCMD_DEV_BUS_DROP |
-		       OTGCMD_DEV_POWER_OFF | OTGCMD_HOST_POWER_OFF,
-		       &cdns->otg_regs->cmd);
-	}
+	if (on)
+		writel(value | vbus, &cdns->otg_regs->ien);
+	else
+		writel(vbus & ~vbus, &cdns->otg_regs->ien);
+}
 
-	return 0;
+static void cdns3_drd_switch_off(struct cdns3 *cdns)
+{
+	/*
+	 * driver should wait at least 10us after disabling Device
+	 * before turning-off Device (DEV_BUS_DROP)
+	 */
+	usleep_range(30, 40);
+	writel(OTGCMD_HOST_BUS_DROP | OTGCMD_DEV_BUS_DROP |
+	       OTGCMD_DEV_POWER_OFF | OTGCMD_HOST_POWER_OFF,
+	       &cdns->otg_regs->cmd);
+	usleep_range(3000, 4000);
 }
 
-/**
- * cdns3_drd_switch_gadget - start/stop gadget
- * @cdns: Pointer to controller context structure
- * @on: 1 for start, 0 for stop
- *
- * Returns 0 on success otherwise negative errno
- */
-static int cdns3_drd_switch_gadget(struct cdns3 *cdns, int on)
+static int cdns3_drd_switch_on(struct cdns3 *cdns)
 {
-	int ret;
-	u32 reg = OTGCMD_OTG_DIS;
+	int ret = 0;
+
+	if (cdns->role == CDNS3_ROLE_GADGET) {
+		if (cdns->pdata && cdns->pdata->set_mode)
+			cdns->pdata->set_mode(cdns);
 
-	/* switch OTG core */
-	if (on) {
-		writel(OTGCMD_DEV_BUS_REQ | reg, &cdns->otg_regs->cmd);
+		writel(OTGCMD_DEV_BUS_REQ | OTGCMD_OTG_DIS,
+			&cdns->otg_regs->cmd);
 
 		dev_dbg(cdns->dev, "Waiting for Device mode is turned on\n");
 
 		ret = cdns3_handshake(&cdns->otg_regs->sts, OTGSTS_DEV_READY,
 				      OTGSTS_DEV_READY, 100000);
+		if (ret)
+			return ret;
+	} else if (cdns->role == CDNS3_ROLE_HOST) {
+		if (cdns->pdata && cdns->pdata->set_mode)
+			cdns->pdata->set_mode(cdns);
+
+		writel(OTGCMD_HOST_BUS_REQ | OTGCMD_OTG_DIS, &cdns->otg_regs->cmd);
 
+		ret = cdns3_handshake(&cdns->otg_regs->sts, OTGSTS_XHCI_READY,
+				      OTGSTS_XHCI_READY, 100000);
+		dev_dbg(cdns->dev, "Waiting for Host mode is turned on, ret=%d\n", ret);
 		if (ret)
 			return ret;
 	} else {
-		/*
-		 * driver should wait at least 10us after disabling Device
-		 * before turning-off Device (DEV_BUS_DROP)
-		 */
-		usleep_range(20, 30);
-		writel(OTGCMD_HOST_BUS_DROP | OTGCMD_DEV_BUS_DROP |
-		       OTGCMD_DEV_POWER_OFF | OTGCMD_HOST_POWER_OFF,
-		       &cdns->otg_regs->cmd);
+		if (cdns->pdata && cdns->pdata->set_mode)
+			cdns->pdata->set_mode(cdns);
 	}
 
-	return 0;
+	return ret;
 }
 
 /**
  * cdns3_init_otg_mode - initialize drd controller
  * @cdns: Pointer to controller context structure
  *
- * Returns 0 on success otherwise negative errno
  */
 static void cdns3_init_otg_mode(struct cdns3 *cdns)
 {
+	u32 reg;
+
 	cdns3_otg_disable_irq(cdns);
 	/* clear all interrupts */
 	writel(~0, &cdns->otg_regs->ivect);
 
-	cdns3_set_mode(cdns, USB_DR_MODE_OTG);
-
-	if (cdns3_is_host(cdns))
-		cdns3_drd_switch_host(cdns, 1);
-	else
-		cdns3_drd_switch_gadget(cdns, 1);
-
-	cdns3_otg_enable_irq(cdns);
-}
+	cdns3_enable_id_irq(cdns);
+	if (cdns3_get_id(cdns) == 0)
+		cdns3_enable_vbus_irq(cdns, true);
 
-/**
- * cdns3_drd_update_mode - initialize mode of operation
- * @cdns: Pointer to controller context structure
- *
- * Returns 0 on success otherwise negative errno
- */
-int cdns3_drd_update_mode(struct cdns3 *cdns)
-{
-	int ret = 0;
-
-	if (cdns->desired_dr_mode == cdns->current_dr_mode)
-		return ret;
-
-	cdns3_drd_switch_gadget(cdns, 0);
-	cdns3_drd_switch_host(cdns, 0);
-
-	switch (cdns->desired_dr_mode) {
-	case USB_DR_MODE_PERIPHERAL:
-		cdns3_set_mode(cdns, USB_DR_MODE_PERIPHERAL);
-		break;
-	case USB_DR_MODE_HOST:
-		cdns3_set_mode(cdns, USB_DR_MODE_HOST);
-		break;
-	case USB_DR_MODE_OTG:
-		cdns3_init_otg_mode(cdns);
-		break;
-	default:
-		dev_err(cdns->dev, "Unsupported mode of operation %d\n",
-			cdns->dr_mode);
-		return -EINVAL;
+	if (cdns->version == CDNS3_CONTROLLER_V1) {
+		reg = readl(&cdns->otg_v1_regs->override);
+		reg |= OVERRIDE_IDPULLUP;
+		writel(reg, &cdns->otg_v1_regs->override);
+	} else {
+		reg = readl(&cdns->otg_v0_regs->ctrl1);
+		reg |= OVERRIDE_IDPULLUP_V0;
+		writel(reg, &cdns->otg_v0_regs->ctrl1);
 	}
 
-	return ret;
+	/*
+	 * Hardware specification says: "ID_VALUE must be valid within
+	 * 50ms after idpullup is set to '1" so driver must wait
+	 * 50ms before reading this pin.
+	 */
+	usleep_range(50000, 60000);
 }
 
 /**
@@ -253,8 +181,9 @@ static irqreturn_t cdns3_drd_irq(int irq, void *data)
 	irqreturn_t ret = IRQ_NONE;
 	struct cdns3 *cdns = data;
 	u32 reg;
+	bool vbus;
 
-	if (cdns->dr_mode != USB_DR_MODE_OTG)
+	if (cdns->dr_mode == USB_DR_MODE_HOST)
 		return ret;
 
 	reg = readl(&cdns->otg_regs->ivect);
@@ -266,20 +195,72 @@ static irqreturn_t cdns3_drd_irq(int irq, void *data)
 		dev_dbg(cdns->dev, "OTG IRQ: new ID: %d\n",
 			cdns3_get_id(cdns));
 
+		writel(OTGIEN_ID_CHANGE_INT, &cdns->otg_regs->ivect);
 		queue_work(system_freezable_wq, &cdns->role_switch_wq);
 
 		ret = IRQ_HANDLED;
 	}
 
-	writel(~0, &cdns->otg_regs->ivect);
+	if ((reg & OTGIEN_VBUSSESS_RISE_INT)
+		|| (reg & OTGIEN_VBUSSESS_FALL_INT)) {
+		u32 vbus_status;
+		vbus = cdns3_get_vbus(cdns);
+		dev_dbg(cdns->dev, "OTG IRQ: VBUS : %d\n", vbus);
+		vbus_status = vbus ? OTGIEN_VBUSSESS_RISE_INT
+			: OTGIEN_VBUSSESS_FALL_INT;
+		writel(vbus_status, &cdns->otg_regs->ivect);
+		queue_work(system_freezable_wq, &cdns->role_switch_wq);
+		ret = IRQ_HANDLED;
+	}
+
 	return ret;
 }
 
-int cdns3_drd_init(struct cdns3 *cdns)
+static int cdns3_extcon_notifier(struct notifier_block *nb, unsigned long event,
+			     void *ptr)
+{
+	struct cdns3 *cdns = container_of(nb, struct cdns3, extcon_nb);
+
+	queue_work(system_freezable_wq, &cdns->role_switch_wq);
+
+	return NOTIFY_DONE;
+}
+
+static int cdns3_register_extcon(struct cdns3 *cdns)
+{
+	struct extcon_dev *extcon;
+	struct device *dev = cdns->dev;
+	int ret;
+
+	if (of_property_read_bool(dev->of_node, "extcon")) {
+		extcon = extcon_get_edev_by_phandle(dev, 0);
+		if (IS_ERR(extcon))
+			return PTR_ERR(extcon);
+
+		ret = devm_extcon_register_notifier(dev, extcon,
+			EXTCON_USB_HOST, &cdns->extcon_nb);
+		if (ret < 0) {
+			dev_err(dev, "register Host Connector failed\n");
+			return ret;
+		}
+
+		ret = devm_extcon_register_notifier(dev, extcon,
+			EXTCON_USB, &cdns->extcon_nb);
+		if (ret < 0) {
+			dev_err(dev, "register Device Connector failed\n");
+			return ret;
+		}
+
+		cdns->extcon = extcon;
+		cdns->extcon_nb.notifier_call = cdns3_extcon_notifier;
+	}
+
+	return 0;
+}
+
+int cdns3_finalize_dr_mode(struct cdns3 *cdns)
 {
 	void __iomem *regs;
-	int ret = 0;
-	u32 state;
 
 	regs = devm_ioremap_resource(cdns->dev, &cdns->otg_res);
 	if (IS_ERR(regs))
@@ -299,6 +280,7 @@ int cdns3_drd_init(struct cdns3 *cdns)
 		cdns->version  = CDNS3_CONTROLLER_V0;
 		cdns->otg_v1_regs = NULL;
 		cdns->otg_regs = regs;
+		writel(0x1, &cdns->otg_v0_regs->simulate);
 		dev_info(cdns->dev, "DRD version v0 (%08x)\n",
 			 readl(&cdns->otg_v0_regs->version));
 	} else {
@@ -311,27 +293,87 @@ int cdns3_drd_init(struct cdns3 *cdns)
 			 readl(&cdns->otg_v1_regs->rid));
 	}
 
-	state = OTGSTS_STRAP(readl(&cdns->otg_regs->sts));
+	cdns->dr_mode = usb_get_dr_mode(cdns->dev);
+	if (cdns->dr_mode == USB_DR_MODE_UNKNOWN)
+		cdns->dr_mode = USB_DR_MODE_OTG;
 
-	/* Update dr_mode according to STRAP configuration. */
-	cdns->dr_mode = USB_DR_MODE_OTG;
-	if (state == OTGSTS_STRAP_HOST) {
-		dev_info(cdns->dev, "Controller strapped to HOST\n");
-		cdns->dr_mode = USB_DR_MODE_HOST;
-	} else if (state == OTGSTS_STRAP_GADGET) {
-		dev_info(cdns->dev, "Controller strapped to PERIPHERAL\n");
-		cdns->dr_mode = USB_DR_MODE_PERIPHERAL;
+	return 0;
+}
+
+static int cdns3_do_role_switch(struct cdns3 *cdns, enum cdns3_roles role)
+{
+	int ret = 0;
+	enum cdns3_roles current_role;
+
+	dev_dbg(cdns->dev, "current role is %d, switch to %d\n",
+			cdns->role, role);
+
+	if (cdns->role == role)
+		return 0;
+
+	current_role = cdns->role;
+	cdns3_role_stop(cdns);
+	/*
+	 * - Enable vbus interrupt after leaving host
+	 * - Disable vbus interrupt before entering host
+	 */
+
+	if (current_role == CDNS3_ROLE_HOST)
+		cdns3_enable_vbus_irq(cdns, true);
+	else if (role == CDNS3_ROLE_HOST)
+		cdns3_enable_vbus_irq(cdns, false);
+	if (role == CDNS3_ROLE_END) {
+		/* TODO: Force B Session Valid as 0 */
+		return 0;
 	}
 
-	cdns->desired_dr_mode = cdns->dr_mode;
-	cdns->current_dr_mode = USB_DR_MODE_UNKNOWN;
+	ret = cdns3_role_start(cdns, role);
+	if (ret) {
+		/* Back to current role */
+		dev_err(cdns->dev, "set %d has failed, back to %d\n",
+					role, current_role);
+		if (role == CDNS3_ROLE_HOST)
+			cdns3_enable_vbus_irq(cdns, true);
+		ret = cdns3_role_start(cdns, current_role);
+	}
 
-	ret = devm_request_threaded_irq(cdns->dev, cdns->irq, cdns3_drd_irq,
-					NULL, IRQF_SHARED,
-					dev_name(cdns->dev), cdns);
+	return ret;
+}
 
-	if (ret)
-		return ret;
+/**
+ * cdns3_role_switch - work queue handler for role switch
+ *
+ * @work: work queue item structure
+ *
+ * Handles below events:
+ * - Role switch for dual-role devices
+ * - CDNS3_ROLE_GADGET <--> CDNS3_ROLE_END for peripheral-only devices
+ */
+static void cdns3_role_switch(struct work_struct *work)
+{
+	struct cdns3 *cdns = container_of(work, struct cdns3,
+			role_switch_wq);
+	bool device, host;
+
+	host = cdns3_is_host(cdns);
+	device = cdns3_is_device(cdns);
+
+	if (host) {
+		if (cdns->roles[CDNS3_ROLE_HOST])
+			cdns3_do_role_switch(cdns, CDNS3_ROLE_HOST);
+		return;
+	}
+
+	if (device)
+		cdns3_do_role_switch(cdns, CDNS3_ROLE_GADGET);
+	else
+		cdns3_do_role_switch(cdns, CDNS3_ROLE_END);
+}
+
+int cdns3_drd_init(struct cdns3 *cdns)
+{
+	int ret;
+	u32 state;
 
 	state = readl(&cdns->otg_regs->sts);
 	if (OTGSTS_OTG_NRDY(state) != 0) {
@@ -339,12 +381,82 @@ int cdns3_drd_init(struct cdns3 *cdns)
 		return -ENODEV;
 	}
 
-	ret = cdns3_drd_update_mode(cdns);
+	INIT_WORK(&cdns->role_switch_wq, cdns3_role_switch);
+	ret = cdns3_register_extcon(cdns);
+	if (ret)
+		return ret;
+
+	cdns3_init_otg_mode(cdns);
+
+	return devm_request_threaded_irq(cdns->dev, cdns->irq, cdns3_drd_irq,
+					NULL, IRQF_SHARED,
+					dev_name(cdns->dev), cdns);
+}
+
+int cdns3_role_start(struct cdns3 *cdns, enum cdns3_roles role)
+{
+	int ret;
+
+	if (role >= CDNS3_ROLE_END) {
+		WARN_ON(role > CDNS3_ROLE_END);
+		return 0;
+	}
+
+	if (!cdns->roles[role])
+		return -ENXIO;
+
+	if (cdns->roles[role]->state == CDNS3_ROLE_STATE_ACTIVE)
+		return 0;
 
+	mutex_lock(&cdns->mutex);
+	cdns->role = role;
+	cdns3_drd_switch_on(cdns);
+	ret = cdns->roles[role]->start(cdns);
+	if (!ret)
+		cdns->roles[role]->state = CDNS3_ROLE_STATE_ACTIVE;
+	mutex_unlock(&cdns->mutex);
 	return ret;
 }
 
-int cdns3_drd_exit(struct cdns3 *cdns)
+void cdns3_role_stop(struct cdns3 *cdns)
+{
+	enum cdns3_roles role = cdns->role;
+
+	if (role >= CDNS3_ROLE_END) {
+		WARN_ON(role > CDNS3_ROLE_END);
+		return;
+	}
+
+	if (cdns->roles[role]->state == CDNS3_ROLE_STATE_INACTIVE)
+		return;
+
+	mutex_lock(&cdns->mutex);
+	cdns->roles[role]->stop(cdns);
+	cdns->roles[role]->state = CDNS3_ROLE_STATE_INACTIVE;
+	cdns3_drd_switch_off(cdns);
+	mutex_unlock(&cdns->mutex);
+}
+
+/*
+ * cdns->role gets from cdns3_get_role, and this API tells role at the
+ * runtime.
+ * If both roles are supported, the role is selected based on vbus/id.
+ * It could be read from OTG register or external connector.
+ * If only single role is supported, only one role structure
+ * is allocated, cdns->roles[CDNS3_ROLE_HOST] or cdns->roles[CDNS3_ROLE_GADGET].
+ */
+enum cdns3_roles cdns3_get_role(struct cdns3 *cdns)
 {
-	return cdns3_drd_switch_host(cdns, 0);
+	if (cdns->roles[CDNS3_ROLE_HOST] && cdns->roles[CDNS3_ROLE_GADGET]) {
+		if (cdns3_is_host(cdns))
+			return CDNS3_ROLE_HOST;
+		else if (cdns3_is_device(cdns))
+			return CDNS3_ROLE_GADGET;
+		else
+			return CDNS3_ROLE_END;
+	} else {
+		return cdns->roles[CDNS3_ROLE_HOST]
+			? CDNS3_ROLE_HOST
+			: CDNS3_ROLE_GADGET;
+	}
 }
diff --git a/drivers/usb/cdns3/drd.h b/drivers/usb/cdns3/drd.h
index 6a29cdcb492d..42f151d2f653 100644
--- a/drivers/usb/cdns3/drd.h
+++ b/drivers/usb/cdns3/drd.h
@@ -11,7 +11,6 @@
 
 #include <linux/usb/otg.h>
 #include <linux/phy/phy.h>
-#include "core.h"
 
 /*  DRD register interface for version v1. */
 struct cdns3_otg_regs {
@@ -100,6 +99,10 @@ struct cdns3_otg_common_regs {
 /* OTGIEN - bitmasks */
 /* ID change interrupt enable */
 #define OTGIEN_ID_CHANGE_INT		BIT(0)
+/* session valid rise detected interrupt enable.*/
+#define OTGIEN_VBUSSESS_RISE_INT	BIT(2)
+/* session valid fall detected interrupt enable */
+#define OTGIEN_VBUSSESS_FALL_INT	BIT(3)
 /* Vbusvalid fall detected interrupt enable.*/
 #define OTGIEN_VBUSVALID_RISE_INT	BIT(4)
 /* Vbusvalid fall detected interrupt enable */
@@ -152,11 +155,10 @@ struct cdns3_otg_common_regs {
 /* Only for CDNS3_CONTROLLER_V0 version */
 #define OVERRIDE_IDPULLUP_V0		BIT(24)
 
-int cdns3_is_host(struct cdns3 *cdns);
-int cdns3_is_device(struct cdns3 *cdns);
-int cdns3_get_id(struct cdns3 *cdns);
 int cdns3_drd_init(struct cdns3 *cdns);
-int cdns3_drd_exit(struct cdns3 *cdns);
-int cdns3_drd_update_mode(struct cdns3 *cdns);
+int cdns3_finalize_dr_mode(struct cdns3 *cdns);
+enum cdns3_roles cdns3_get_role(struct cdns3 *cdns);
+int cdns3_role_start(struct cdns3 *cdns, enum cdns3_roles role);
+void cdns3_role_stop(struct cdns3 *cdns);
 
 #endif /* __LINUX_CDNS3_DRD */
diff --git a/drivers/usb/cdns3/gadget.c b/drivers/usb/cdns3/gadget.c
index 0d95eb00be37..5e04d7740087 100644
--- a/drivers/usb/cdns3/gadget.c
+++ b/drivers/usb/cdns3/gadget.c
@@ -498,6 +498,9 @@ int cdns3_ep_run_transfer(struct cdns3_endpoint *priv_ep,
 	int  num_trb;
 	int address;
 	int pcs;
+	bool doorbell;
+	int dma_index;
+	u32 reg;
 
 	if (priv_ep->type == USB_ENDPOINT_XFER_ISOC)
 		num_trb = priv_ep->interval;
@@ -541,6 +544,28 @@ int cdns3_ep_run_transfer(struct cdns3_endpoint *priv_ep,
 	}
 
 	first_pcs = priv_ep->pcs ? TRB_CYCLE : 0;
+	/* arm transfer on selected endpoint */
+	cdns3_select_ep(priv_ep->cdns3_dev, address);
+
+	doorbell = !!(readl(&priv_dev->regs->ep_cmd) & EP_CMD_DRDY);
+	dma_index = (readl(&priv_dev->regs->ep_traddr) -
+			 priv_ep->trb_pool_dma) / TRB_SIZE;
+
+	if (!priv_ep->wa_set) {
+		if (doorbell & ((priv_ep->enqueue == 0
+				     && dma_index >= TRBS_PER_SEGMENT - 2) ||
+			(dma_index == priv_ep->enqueue) ||
+			(dma_index == priv_ep->enqueue - 1))) {
+			priv_ep->wa_cycle_bit = first_pcs;
+			priv_ep->wa_set = 1;
+			priv_ep->wa_trb = trb;
+			first_pcs ^= 0x1;
+			reg = readl(&priv_dev->regs->ep_sts_en);
+			reg |= EP_STS_TRBERR;
+			writel(reg, &priv_dev->regs->ep_sts_en);
+		}
+	}
+
 
 	do {
 	/* fill TRB */
@@ -583,7 +608,6 @@ int cdns3_ep_run_transfer(struct cdns3_endpoint *priv_ep,
 	wmb();
 
 	priv_req->flags |= REQUEST_PENDING;
-
 	/* give the TD to the consumer*/
 	if (sg_iter == 1)
 		trb->control |= first_pcs | TRB_IOC | TRB_ISP;
@@ -593,9 +617,6 @@ int cdns3_ep_run_transfer(struct cdns3_endpoint *priv_ep,
 	trace_cdns3_prepare_trb(priv_ep, priv_req->trb);
 	trace_cdns3_ring(priv_ep);
 
-	/* arm transfer on selected endpoint */
-	cdns3_select_ep(priv_ep->cdns3_dev, address);
-
 	/*
 	 * For DMULT mode we can set address to transfer ring only once after
 	 * enabling endpoint.
@@ -607,10 +628,12 @@ int cdns3_ep_run_transfer(struct cdns3_endpoint *priv_ep,
 	}
 
 	/*clearing TRBERR and EP_STS_DESCMIS before seting DRDY*/
-	writel(EP_STS_TRBERR | EP_STS_DESCMIS, &priv_dev->regs->ep_sts);
-	trace_cdns3_doorbell_epx(priv_ep->name,
-				 readl(&priv_dev->regs->ep_traddr));
-	writel(EP_CMD_DRDY, &priv_dev->regs->ep_cmd);
+	if (!priv_ep->wa_set) {
+		writel(EP_STS_TRBERR | EP_STS_DESCMIS, &priv_dev->regs->ep_sts);
+		writel(EP_CMD_DRDY, &priv_dev->regs->ep_cmd);
+		trace_cdns3_doorbell_epx(priv_ep->name,
+					 readl(&priv_dev->regs->ep_traddr));
+	}
 
 	return 0;
 }
@@ -852,6 +875,28 @@ static int cdns3_check_ep_interrupt_proceed(struct cdns3_endpoint *priv_ep)
 	ep_sts_reg = readl(&priv_dev->regs->ep_sts);
 	writel(ep_sts_reg, &priv_dev->regs->ep_sts);
 
+	if (ep_sts_reg & EP_STS_TRBERR) {
+		u32 reg;
+		if(!priv_ep->wa_set) {
+			printk("not wa_set at TRBERR\n");
+		} else {
+			printk(KERN_DEBUG "wa_set at TRBERR\n");
+			priv_ep->wa_set = 0;
+			if(priv_ep->wa_cycle_bit) {
+				priv_ep->wa_trb->control =
+					priv_ep->wa_trb->control | 0x1;
+			} else {
+				priv_ep->wa_trb->control =
+					priv_ep->wa_trb->control & ~0x1;
+			}
+
+			reg = readl(&priv_dev->regs->ep_sts_en);
+			reg &= ~EP_STS_TRBERR;
+			writel(reg, &priv_dev->regs->ep_sts_en);
+			writel(EP_CMD_DRDY, &priv_dev->regs->ep_cmd);
+		}
+	}
+
 	if ((ep_sts_reg & EP_STS_IOC) || (ep_sts_reg & EP_STS_ISP)) {
 		if (priv_ep->flags & EP_QUIRK_EXTRA_BUF_EN) {
 			if (ep_sts_reg & EP_STS_ISP)
@@ -951,6 +996,9 @@ static irqreturn_t cdns3_device_irq_handler(int irq, void *data)
 	unsigned long flags;
 	u32 reg;
 
+	if (cdns->role != CDNS3_ROLE_GADGET)
+		return ret;
+
 	priv_dev = cdns->gadget_dev;
 	spin_lock_irqsave(&priv_dev->lock, flags);
 
@@ -1249,7 +1297,7 @@ static int cdns3_gadget_ep_enable(struct usb_ep *ep,
 {
 	struct cdns3_endpoint *priv_ep;
 	struct cdns3_device *priv_dev;
-	u32 reg = EP_STS_EN_TRBERREN;
+	u32 reg = 0/*EP_STS_EN_TRBERREN*/;
 	u32 bEndpointAddress;
 	unsigned long flags;
 	int ret;
@@ -1311,8 +1359,9 @@ static int cdns3_gadget_ep_enable(struct usb_ep *ep,
 	 * if before first DESCMISS interrupt the DMA will be armed.
 	 */
 	if (!priv_ep->dir && priv_ep->type != USB_ENDPOINT_XFER_ISOC) {
-		priv_ep->flags |= EP_QUIRK_EXTRA_BUF_DET;
+	/*	priv_ep->flags |= EP_QUIRK_EXTRA_BUF_DET;
 		reg |= EP_STS_EN_DESCMISEN;
+		*/
 	}
 
 	writel(reg, &priv_dev->regs->ep_sts_en);
@@ -1325,6 +1374,7 @@ static int cdns3_gadget_ep_enable(struct usb_ep *ep,
 	priv_ep->flags |= EP_ENABLED | EP_UPDATE_EP_TRBADDR;
 	priv_ep->enqueue = 0;
 	priv_ep->dequeue = 0;
+	priv_ep->wa_set = 0;
 	reg = readl(&priv_dev->regs->ep_sts);
 	priv_ep->pcs = !!EP_STS_CCS(reg);
 	priv_ep->ccs = !!EP_STS_CCS(reg);
diff --git a/drivers/usb/cdns3/gadget.h b/drivers/usb/cdns3/gadget.h
index 41cec7f085ad..a0bce149726e 100644
--- a/drivers/usb/cdns3/gadget.h
+++ b/drivers/usb/cdns3/gadget.h
@@ -897,7 +897,7 @@ struct cdns3_usb_regs {
 /*
  * USBSS-DEV DMA interface.
  */
-#define TRBS_PER_SEGMENT	40
+#define TRBS_PER_SEGMENT       40
 
 #define ISO_MAX_INTERVAL	10
 
@@ -1063,6 +1063,9 @@ struct cdns3_endpoint {
 	u8			ccs;
 	int			enqueue;
 	int			dequeue;
+	int 			wa_cycle_bit;
+	bool			wa_set;
+	struct cdns3_trb 	*wa_trb;
 };
 
 /**
diff --git a/drivers/usb/cdns3/trace.h b/drivers/usb/cdns3/trace.h
index f3d7a91fae86..8e97575a6b01 100644
--- a/drivers/usb/cdns3/trace.h
+++ b/drivers/usb/cdns3/trace.h
@@ -179,7 +179,7 @@ DECLARE_EVENT_CLASS(cdns3_log_request,
 		__entry->flags = req->flags;
 	),
 	TP_printk("%s: req: %p, length: %u/%u %s%s%s, status: %d,"
-		  " trb: [start:%d, end:%d: virt addr %pa], flags:%x ",
+		  " trb: [start:%d, end:%d: virt addr 0x%p], flags:%x ",
 		__get_str(name), __entry->req, __entry->actual, __entry->length,
 		__entry->zero ? "zero | " : "",
 		__entry->short_not_ok ? "short | " : "",
@@ -236,7 +236,7 @@ DECLARE_EVENT_CLASS(cdns3_log_trb,
 		__entry->control = trb->control;
 		__entry->type = usb_endpoint_type(priv_ep->endpoint.desc);
 	),
-	TP_printk("%s: trb 0x%pa, dma buf: 0x%08x, size: %ld, ctrl: 0x%08x (%s%s%s%s%s%s%s)",
+	TP_printk("%s: trb 0x%p, dma buf: 0x%08x, size: %ld, ctrl: 0x%08x (%s%s%s%s%s%s%s)",
 		__get_str(name), __entry->trb, __entry->buffer,
 		TRB_LEN(__entry->length), __entry->control,
 		__entry->control & TRB_CYCLE ? "C=1, " : "C=0, ",
-- 
2.14.1

