[U-Boot,v2,1/4] USB: gadget: Add the cadence USB3 gadget driver
diff mbox series

Message ID 1565784996-402-2-git-send-email-sherry.sun@nxp.com
State Superseded
Delegated to: Marek Vasut
Headers show
Series
  • USB: Add cadence USB3 gadget driver and host driver
Related show

Commit Message

Sherry Sun Aug. 14, 2019, 12:16 p.m. UTC
From: Sherry Sun <sherry.sun@nxp.com>

This driver is ported from NXP i.MX U-Boot version imx_v2019.04
and some changes have also been made to adapt to U-Boot.

Add the Cadence USB3 IP(CDNS3) driver for the gadget (device mode).
The CDNS3 gadget driver support DM mode. CONFIG_DM_USB_GADGET should
be enabled when use this driver. And gadget_is_cdns3 checking is
added to provide bcdUSB value in device descriptor.

Signed-off-by: Sherry Sun <sherry.sun@nxp.com>
---
 Makefile                                   |    1 +
 doc/device-tree-bindings/usb/cdns-usb3.txt |   39 +
 drivers/usb/Kconfig                        |    2 +
 drivers/usb/cdns3/Kconfig                  |   20 +
 drivers/usb/cdns3/Makefile                 |    5 +
 drivers/usb/cdns3/cdns3-generic.c          |  115 +
 drivers/usb/cdns3/cdns3-nxp-reg-def.h      |   89 +
 drivers/usb/cdns3/core.c                   |  197 ++
 drivers/usb/cdns3/core.h                   |  118 ++
 drivers/usb/cdns3/dev-regs-macro.h         |  116 +
 drivers/usb/cdns3/dev-regs-map.h           |  117 ++
 drivers/usb/cdns3/gadget-export.h          |   26 +
 drivers/usb/cdns3/gadget.c                 | 2222 ++++++++++++++++++++
 drivers/usb/cdns3/gadget.h                 |  225 ++
 drivers/usb/cdns3/io.h                     |   30 +
 drivers/usb/gadget/epautoconf.c            |    4 +
 drivers/usb/gadget/gadget_chips.h          |    7 +
 drivers/usb/gadget/udc/Makefile            |    1 +
 include/linux/usb/gadget.h                 |    3 +
 scripts/Makefile.spl                       |    1 +
 20 files changed, 3338 insertions(+)
 create mode 100644 doc/device-tree-bindings/usb/cdns-usb3.txt
 create mode 100644 drivers/usb/cdns3/Kconfig
 create mode 100644 drivers/usb/cdns3/Makefile
 create mode 100644 drivers/usb/cdns3/cdns3-generic.c
 create mode 100644 drivers/usb/cdns3/cdns3-nxp-reg-def.h
 create mode 100644 drivers/usb/cdns3/core.c
 create mode 100644 drivers/usb/cdns3/core.h
 create mode 100644 drivers/usb/cdns3/dev-regs-macro.h
 create mode 100644 drivers/usb/cdns3/dev-regs-map.h
 create mode 100644 drivers/usb/cdns3/gadget-export.h
 create mode 100644 drivers/usb/cdns3/gadget.c
 create mode 100644 drivers/usb/cdns3/gadget.h
 create mode 100644 drivers/usb/cdns3/io.h

Comments

Marek Vasut Aug. 14, 2019, 3:26 p.m. UTC | #1
On 8/14/19 2:16 PM, sherry sun wrote:
> From: Sherry Sun <sherry.sun@nxp.com>
> 
> This driver is ported from NXP i.MX U-Boot version imx_v2019.04
> and some changes have also been made to adapt to U-Boot.
> 
> Add the Cadence USB3 IP(CDNS3) driver for the gadget (device mode).
> The CDNS3 gadget driver support DM mode. CONFIG_DM_USB_GADGET should
> be enabled when use this driver. And gadget_is_cdns3 checking is
> added to provide bcdUSB value in device descriptor.

The cadence core isn't xhci compatible ? Sigh ...

[...]

> +++ b/doc/device-tree-bindings/usb/cdns-usb3.txt
> @@ -0,0 +1,39 @@
> +* Cadence USB3 Controller
> +
> +Required properties:
> +- compatible: "Cadence,usb3";

cdns , no ?

[...]

> +static int cdns3_generic_peripheral_clk_init(struct udevice *dev,
> +					     struct cdns3_generic_peripheral
> +					     *priv)
> +{
> +	int ret;
> +
> +	ret = clk_get_bulk(dev, &priv->clks);
> +	if (ret)
> +		return ret;
> +
> +#if CONFIG_IS_ENABLED(CLK)

Why is the ifdef protecting only half of the clock functions ?

> +	ret = clk_enable_bulk(&priv->clks);
> +	if (ret) {
> +		clk_release_bulk(&priv->clks);
> +		return ret;
> +	}
> +#endif
> +
> +	return 0;
> +}


[...]

> +static void cdns3_set_role(struct cdns3 *cdns, enum cdns3_roles role)
> +{
> +	u32 value;
> +	int timeout_us = 100000;
> +	struct cdns3_generic_peripheral *priv = container_of(cdns,
> +			struct cdns3_generic_peripheral, cdns3);
> +
> +	if (role == CDNS3_ROLE_END)
> +		return;
> +
> +	/* Wait clk value */
> +	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> +	writel(value, cdns->none_core_regs + USB3_SSPHY_STATUS);
> +	udelay(1);
> +	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> +	while ((value & 0xf0000000) != 0xf0000000 && timeout_us-- > 0) {
> +		value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> +		udelay(1);
> +	}

Is this like wait_for_bit() ?

> +	if (timeout_us <= 0)
> +		dev_err(cdns->dev, "wait clkvld timeout\n");
> +
> +	/* Set all Reset bits */
> +	setbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1, ALL_SW_RESET);
> +	udelay(1);
> +
> +	if (role == CDNS3_ROLE_HOST) {

Do we need custom controller role definition ? I don't think so, just
use the one in the USB stack.

Also, use clrsetbits_le32()

> +		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
> +		value = (value & ~MODE_STRAP_MASK) | HOST_MODE | OC_DISABLE;
> +		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
> +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> +			     PHYAHB_SW_RESET);
> +		mdelay(1);
> +		generic_phy_init(&priv->phy);
> +		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
> +			     RXDET_IN_P3_32KHZ);
> +		udelay(10);
> +		/* Force B Session Valid as 1 */
> +		writel(0x0060, cdns->phy_regs + 0x380a4);
> +		mdelay(1);
> +
> +		setbits_le32(cdns->none_core_regs + USB3_INT_REG, HOST_INT1_EN);
> +
> +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> +			     ALL_SW_RESET);
> +
> +		dev_dbg(cdns->dev, "wait xhci_power_on_ready\n");
> +
> +		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> +		timeout_us = 100000;
> +		while (!(value & HOST_POWER_ON_READY) && timeout_us-- > 0) {
> +			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> +			udelay(1);
> +		}
> +
> +		if (timeout_us <= 0)
> +			dev_err(cdns->dev, "wait xhci_power_on_ready timeout\n");
> +
> +		dev_dbg(cdns->dev, "switch to host role successfully\n");
> +	} else { /* gadget mode */

Split this into sensibly long funtions please.

> +		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
> +		value = (value & ~MODE_STRAP_MASK) | DEV_MODE;
> +		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
> +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> +			     PHYAHB_SW_RESET);
> +
> +		generic_phy_init(&priv->phy);
> +		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
> +			     RXDET_IN_P3_32KHZ);
> +		udelay(10);
> +		/* Force B Session Valid as 1 */
> +		writel(0x0060, cdns->phy_regs + 0x380a4);
> +		setbits_le32(cdns->none_core_regs + USB3_INT_REG, DEV_INT_EN);
> +
> +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> +			     ALL_SW_RESET);
> +
> +		dev_dbg(cdns->dev, "wait gadget_power_on_ready\n");
> +
> +		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> +		timeout_us = 100000;
> +		while (!(value & DEV_POWER_ON_READY) && timeout_us-- > 0) {
> +			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> +			udelay(1);
> +		}

wait_for_bit()

[...]

> +/* macros for field CFGSTS */
> +#define USB_STS__CFGSTS__MASK                                       0x00000001U
> +#define USB_STS__USBSPEED__READ(src)     (((uint32_t)(src) & 0x00000070U) >> 4)

u32, fix globally

[...]

> +static void __cdns3_gadget_start(struct usb_ss_dev *usb_ss)
> +{
> +	u32 usb_conf_reg = 0;
> +
> +	/* configure endpoint 0 hardware */
> +	cdns_ep0_config(usb_ss);
> +
> +	/* enable interrupts for endpoint 0 (in and out) */
> +	cdns_writel(&usb_ss->regs->ep_ien,
> +		      EP_IEN__EOUTEN0__MASK | EP_IEN__EINEN0__MASK);
> +
> +	/* enable interrupt for device */
> +	cdns_writel(&usb_ss->regs->usb_ien,
> +		      USB_IEN__U2RESIEN__MASK
> +		      | USB_ISTS__DIS2I__MASK
> +		      | USB_IEN__CON2IEN__MASK
> +		      | USB_IEN__UHRESIEN__MASK
> +		      | USB_IEN__UWRESIEN__MASK
> +		      | USB_IEN__DISIEN__MASK
> +		      | USB_IEN__CONIEN__MASK
> +		      | USB_IEN__U3EXTIEN__MASK
> +		      | USB_IEN__L2ENTIEN__MASK
> +		      | USB_IEN__L2EXTIEN__MASK);
> +
> +	usb_conf_reg = USB_CONF__CLK2OFFDS__MASK |
> +			USB_CONF__L1DS__MASK;
> +	if (usb_ss->gadget.max_speed == USB_SPEED_HIGH)
> +		usb_conf_reg |= USB_CONF__USB3DIS__MASK;
> +	cdns_writel(&usb_ss->regs->usb_conf, usb_conf_reg);
> +
> +	cdns_writel(&usb_ss->regs->usb_conf,
> +		      USB_CONF__U1DS__MASK
> +		      | USB_CONF__U2DS__MASK
> +			/*
> +			 * TODO:
> +			 * | USB_CONF__L1EN__MASK
> +			 */
> +			);

Fix the TODO ?
Sherry Sun Aug. 16, 2019, 4:45 a.m. UTC | #2
Hi Marek

> 
> On 8/14/19 2:16 PM, sherry sun wrote:
> > From: Sherry Sun <sherry.sun@nxp.com>
> >
> > This driver is ported from NXP i.MX U-Boot version imx_v2019.04 and
> > some changes have also been made to adapt to U-Boot.
> >
> > Add the Cadence USB3 IP(CDNS3) driver for the gadget (device mode).
> > The CDNS3 gadget driver support DM mode. CONFIG_DM_USB_GADGET
> should
> > be enabled when use this driver. And gadget_is_cdns3 checking is added
> > to provide bcdUSB value in device descriptor.
> 
> The cadence core isn't xhci compatible ? Sigh ...
> 

Actually, I'm not very understand what the xhci compatible means?
The cadence core can support usb peripheral and host. But for now,
we just done the gadget part in the core code. If it needed later, we 
can add the host part to it.

> [...]
> 
> > +++ b/doc/device-tree-bindings/usb/cdns-usb3.txt
> > @@ -0,0 +1,39 @@
> > +* Cadence USB3 Controller
> > +
> > +Required properties:
> > +- compatible: "Cadence,usb3";
> 
> cdns , no ?

Yes, I have already changed all compatible to "cdns,usb3".

> 
> [...]
> 
> > +static int cdns3_generic_peripheral_clk_init(struct udevice *dev,
> > +					     struct cdns3_generic_peripheral
> > +					     *priv)
> > +{
> > +	int ret;
> > +
> > +	ret = clk_get_bulk(dev, &priv->clks);
> > +	if (ret)
> > +		return ret;
> > +
> > +#if CONFIG_IS_ENABLED(CLK)
> 
> Why is the ifdef protecting only half of the clock functions ?

Because the clk_get_bulk() also has function define even if CONFIG_CLK is not defined.
So this way would not cause undefined reference error.
But for the sake of uniformity, I will move clk_get_bulk() into the ifdef protecting.

> 
> > +	ret = clk_enable_bulk(&priv->clks);
> > +	if (ret) {
> > +		clk_release_bulk(&priv->clks);
> > +		return ret;
> > +	}
> > +#endif
> > +
> > +	return 0;
> > +}
> 
> 
> [...]
> 
> > +static void cdns3_set_role(struct cdns3 *cdns, enum cdns3_roles role)
> > +{
> > +	u32 value;
> > +	int timeout_us = 100000;
> > +	struct cdns3_generic_peripheral *priv = container_of(cdns,
> > +			struct cdns3_generic_peripheral, cdns3);
> > +
> > +	if (role == CDNS3_ROLE_END)
> > +		return;
> > +
> > +	/* Wait clk value */
> > +	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> > +	writel(value, cdns->none_core_regs + USB3_SSPHY_STATUS);
> > +	udelay(1);
> > +	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> > +	while ((value & 0xf0000000) != 0xf0000000 && timeout_us-- > 0) {
> > +		value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
> > +		udelay(1);
> > +	}
> 
> Is this like wait_for_bit() ?

Yes, thanks for the reminder, I have already use wait_for_bit_le32() to replace these.

> 
> > +	if (timeout_us <= 0)
> > +		dev_err(cdns->dev, "wait clkvld timeout\n");
> > +
> > +	/* Set all Reset bits */
> > +	setbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> ALL_SW_RESET);
> > +	udelay(1);
> > +
> > +	if (role == CDNS3_ROLE_HOST) {
> 
> Do we need custom controller role definition ? I don't think so, just use the
> one in the USB stack.
> 

Actually we need create an array with two elements to store the corresponding host/gadge 
function pointer in struct cdns3. And use CDNS3_ROLE_END to do the check.
If we use the role define in USB stack, the array[USB_DR_MODE_UNKNOWN] will always be 
defined but not used. So I think the controller role definition is needed here. 
But if you really think the controller role definition isn’t appropriate, I will change it.

 10 enum cdns3_roles {
 11     CDNS3_ROLE_HOST = 0,
 12     CDNS3_ROLE_GADGET,
 13     CDNS3_ROLE_END,
 14 };

 12 enum usb_dr_mode {
 13     USB_DR_MODE_UNKNOWN,
 14     USB_DR_MODE_HOST,
 15     USB_DR_MODE_PERIPHERAL,
 16     USB_DR_MODE_OTG,
 17 };

> Also, use clrsetbits_le32()
Sure.

> 
> > +		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
> > +		value = (value & ~MODE_STRAP_MASK) | HOST_MODE |
> OC_DISABLE;
> > +		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
> > +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> > +			     PHYAHB_SW_RESET);
> > +		mdelay(1);
> > +		generic_phy_init(&priv->phy);
> > +		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
> > +			     RXDET_IN_P3_32KHZ);
> > +		udelay(10);
> > +		/* Force B Session Valid as 1 */
> > +		writel(0x0060, cdns->phy_regs + 0x380a4);
> > +		mdelay(1);
> > +
> > +		setbits_le32(cdns->none_core_regs + USB3_INT_REG,
> HOST_INT1_EN);
> > +
> > +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> > +			     ALL_SW_RESET);
> > +
> > +		dev_dbg(cdns->dev, "wait xhci_power_on_ready\n");
> > +
> > +		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> > +		timeout_us = 100000;
> > +		while (!(value & HOST_POWER_ON_READY) && timeout_us-- > 0) {
> > +			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> > +			udelay(1);
> > +		}
> > +
> > +		if (timeout_us <= 0)
> > +			dev_err(cdns->dev, "wait xhci_power_on_ready timeout\n");
> > +
> > +		dev_dbg(cdns->dev, "switch to host role successfully\n");
> > +	} else { /* gadget mode */
> 
> Split this into sensibly long funtions please.

Yes, I have done this.

> 
> > +		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
> > +		value = (value & ~MODE_STRAP_MASK) | DEV_MODE;
> > +		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
> > +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> > +			     PHYAHB_SW_RESET);
> > +
> > +		generic_phy_init(&priv->phy);
> > +		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
> > +			     RXDET_IN_P3_32KHZ);
> > +		udelay(10);
> > +		/* Force B Session Valid as 1 */
> > +		writel(0x0060, cdns->phy_regs + 0x380a4);
> > +		setbits_le32(cdns->none_core_regs + USB3_INT_REG, DEV_INT_EN);
> > +
> > +		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
> > +			     ALL_SW_RESET);
> > +
> > +		dev_dbg(cdns->dev, "wait gadget_power_on_ready\n");
> > +
> > +		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> > +		timeout_us = 100000;
> > +		while (!(value & DEV_POWER_ON_READY) && timeout_us-- > 0) {
> > +			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
> > +			udelay(1);
> > +		}
> 
> wait_for_bit()
> 
> [...]
> 
> > +/* macros for field CFGSTS */
> > +#define USB_STS__CFGSTS__MASK
> 0x00000001U
> > +#define USB_STS__USBSPEED__READ(src)     (((uint32_t)(src) &
> 0x00000070U) >> 4)
> 
> u32, fix globally

Yes, I have fixed this.

> 
> [...]
> 
> > +static void __cdns3_gadget_start(struct usb_ss_dev *usb_ss) {
> > +	u32 usb_conf_reg = 0;
> > +
> > +	/* configure endpoint 0 hardware */
> > +	cdns_ep0_config(usb_ss);
> > +
> > +	/* enable interrupts for endpoint 0 (in and out) */
> > +	cdns_writel(&usb_ss->regs->ep_ien,
> > +		      EP_IEN__EOUTEN0__MASK | EP_IEN__EINEN0__MASK);
> > +
> > +	/* enable interrupt for device */
> > +	cdns_writel(&usb_ss->regs->usb_ien,
> > +		      USB_IEN__U2RESIEN__MASK
> > +		      | USB_ISTS__DIS2I__MASK
> > +		      | USB_IEN__CON2IEN__MASK
> > +		      | USB_IEN__UHRESIEN__MASK
> > +		      | USB_IEN__UWRESIEN__MASK
> > +		      | USB_IEN__DISIEN__MASK
> > +		      | USB_IEN__CONIEN__MASK
> > +		      | USB_IEN__U3EXTIEN__MASK
> > +		      | USB_IEN__L2ENTIEN__MASK
> > +		      | USB_IEN__L2EXTIEN__MASK);
> > +
> > +	usb_conf_reg = USB_CONF__CLK2OFFDS__MASK |
> > +			USB_CONF__L1DS__MASK;
> > +	if (usb_ss->gadget.max_speed == USB_SPEED_HIGH)
> > +		usb_conf_reg |= USB_CONF__USB3DIS__MASK;
> > +	cdns_writel(&usb_ss->regs->usb_conf, usb_conf_reg);
> > +
> > +	cdns_writel(&usb_ss->regs->usb_conf,
> > +		      USB_CONF__U1DS__MASK
> > +		      | USB_CONF__U2DS__MASK
> > +			/*
> > +			 * TODO:
> > +			 * | USB_CONF__L1EN__MASK
> > +			 */
> > +			);
> 
> Fix the TODO ?

I have already fixed this.

Best regards 
Sherry sun
Marek Vasut Aug. 16, 2019, 8:28 a.m. UTC | #3
On 8/16/19 6:45 AM, Sherry Sun wrote:
> Hi Marek
> 
>>
>> On 8/14/19 2:16 PM, sherry sun wrote:
>>> From: Sherry Sun <sherry.sun@nxp.com>
>>>
>>> This driver is ported from NXP i.MX U-Boot version imx_v2019.04 and
>>> some changes have also been made to adapt to U-Boot.
>>>
>>> Add the Cadence USB3 IP(CDNS3) driver for the gadget (device mode).
>>> The CDNS3 gadget driver support DM mode. CONFIG_DM_USB_GADGET
>> should
>>> be enabled when use this driver. And gadget_is_cdns3 checking is added
>>> to provide bcdUSB value in device descriptor.
>>
>> The cadence core isn't xhci compatible ? Sigh ...
>>
> 
> Actually, I'm not very understand what the xhci compatible means?
> The cadence core can support usb peripheral and host. But for now,
> we just done the gadget part in the core code. If it needed later, we 
> can add the host part to it.

So this is all needed to support gadget on the cadence3 ?
Sherry Sun Aug. 17, 2019, 8:13 a.m. UTC | #4
Hi Marek

> 
> On 8/16/19 6:45 AM, Sherry Sun wrote:
> > Hi Marek
> >
> >>
> >> On 8/14/19 2:16 PM, sherry sun wrote:
> >>> From: Sherry Sun <sherry.sun@nxp.com>
> >>>
> >>> This driver is ported from NXP i.MX U-Boot version imx_v2019.04 and
> >>> some changes have also been made to adapt to U-Boot.
> >>>
> >>> Add the Cadence USB3 IP(CDNS3) driver for the gadget (device mode).
> >>> The CDNS3 gadget driver support DM mode. CONFIG_DM_USB_GADGET
> >> should
> >>> be enabled when use this driver. And gadget_is_cdns3 checking is
> >>> added to provide bcdUSB value in device descriptor.
> >>
> >> The cadence core isn't xhci compatible ? Sigh ...
> >>
> >
> > Actually, I'm not very understand what the xhci compatible means?
> > The cadence core can support usb peripheral and host. But for now, we
> > just done the gadget part in the core code. If it needed later, we can
> > add the host part to it.
> 
> So this is all needed to support gadget on the cadence3 ?

Yes, this patch is all needed for cdns3 usb gadget driver.
I have already validated this on i.MX8 platform.

Patch
diff mbox series

diff --git a/Makefile b/Makefile
index 8513db94e3..fab1220114 100644
--- a/Makefile
+++ b/Makefile
@@ -728,6 +728,7 @@  libs-$(CONFIG_SYS_FSL_DDR) += drivers/ddr/fsl/
 libs-$(CONFIG_SYS_FSL_MMDC) += drivers/ddr/fsl/
 libs-$(CONFIG_$(SPL_)ALTERA_SDRAM) += drivers/ddr/altera/
 libs-y += drivers/serial/
+libs-y += drivers/usb/cdns3/
 libs-y += drivers/usb/dwc3/
 libs-y += drivers/usb/common/
 libs-y += drivers/usb/emul/
diff --git a/doc/device-tree-bindings/usb/cdns-usb3.txt b/doc/device-tree-bindings/usb/cdns-usb3.txt
new file mode 100644
index 0000000000..613a460e7b
--- /dev/null
+++ b/doc/device-tree-bindings/usb/cdns-usb3.txt
@@ -0,0 +1,39 @@ 
+* Cadence USB3 Controller
+
+Required properties:
+- compatible: "Cadence,usb3";
+- reg: base address and length of the registers
+- interrupts: interrupt for the USB controller
+- interrupt-parent: the interrupt parent for this module
+- clocks: reference to the USB clock
+- clock-names: the name of clocks
+- phys: reference to the USB PHY
+
+Optional properties:
+- dr_mode: One of "host", "peripheral" or "otg". Defaults to "otg"
+- extcon: extcon phandler for cdns3 device
+- power-domains: the power domain for cdns3 controller and phy
+
+Examples:
+
+usbotg3: cdns3@5b110000 {
+	compatible = "Cadence,usb3";
+	reg = <0x0 0x5B110000 0x0 0x10000>,
+		<0x0 0x5B130000 0x0 0x10000>,
+		<0x0 0x5B140000 0x0 0x10000>,
+		<0x0 0x5B160000 0x0 0x40000>;
+	interrupt-parent = <&gic>;
+	interrupts = <GIC_SPI 271 IRQ_TYPE_LEVEL_HIGH>;
+	clocks = <&clk IMX8QM_USB3_LPM_CLK>,
+		<&clk IMX8QM_USB3_BUS_CLK>,
+		<&clk IMX8QM_USB3_ACLK>,
+		<&clk IMX8QM_USB3_IPG_CLK>,
+		<&clk IMX8QM_USB3_CORE_PCLK>;
+	clock-names = "usb3_lpm_clk", "usb3_bus_clk", "usb3_aclk",
+		"usb3_ipg_clk", "usb3_core_pclk";
+	power-domains = <&pd_conn_usb2>;
+	phys = <&usbphy1>;
+	dr_mode = "otg";
+	extcon = <&typec_ptn5150>;
+	status = "disabled";
+};
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index 3b53bf2c58..98f5e936e5 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -70,6 +70,8 @@  source "drivers/usb/host/Kconfig"
 
 source "drivers/usb/dwc3/Kconfig"
 
+source "drivers/usb/cdns3/Kconfig"
+
 source "drivers/usb/musb/Kconfig"
 
 source "drivers/usb/musb-new/Kconfig"
diff --git a/drivers/usb/cdns3/Kconfig b/drivers/usb/cdns3/Kconfig
new file mode 100644
index 0000000000..11a7144b05
--- /dev/null
+++ b/drivers/usb/cdns3/Kconfig
@@ -0,0 +1,20 @@ 
+config USB_CDNS3
+	tristate "Cadence USB3 Dual-Role Controller"
+    depends on (USB && USB_GADGET)
+	help
+	  Say Y here if your system has a cadence USB3 dual-role controller.
+	  It supports: dual-role switch Host-only, and Peripheral-only.
+
+	  When compiled dynamically, the module will be called cdns3.ko.
+
+if USB_CDNS3
+
+config USB_CDNS3_GADGET
+	bool "Cadence USB3 device controller"
+	depends on USB_GADGET
+    select USB_GADGET_DUALSPEED
+	help
+	  Say Y here to enable device controller functionality of the
+	  cadence usb3 driver.
+
+endif
diff --git a/drivers/usb/cdns3/Makefile b/drivers/usb/cdns3/Makefile
new file mode 100644
index 0000000000..374fa06efa
--- /dev/null
+++ b/drivers/usb/cdns3/Makefile
@@ -0,0 +1,5 @@ 
+obj-$(CONFIG_USB_CDNS3)		+= cdns3.o
+
+cdns3-y				:= core.o
+cdns3-$(CONFIG_USB_CDNS3_GADGET)	+= gadget.o
+cdns3-$(CONFIG_$(SPL_)DM_USB_GADGET)	+= cdns3-generic.o
diff --git a/drivers/usb/cdns3/cdns3-generic.c b/drivers/usb/cdns3/cdns3-generic.c
new file mode 100644
index 0000000000..5f17bfd757
--- /dev/null
+++ b/drivers/usb/cdns3/cdns3-generic.c
@@ -0,0 +1,115 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 NXP
+ */
+
+#include <common.h>
+#include <asm-generic/io.h>
+#include <dm.h>
+#include <dm/device-internal.h>
+#include <dm/lists.h>
+#include <malloc.h>
+#include <usb.h>
+#include <clk.h>
+#include <generic-phy.h>
+#include <linux/usb/gadget.h>
+#include "core.h"
+#include "gadget.h"
+
+static int cdns3_generic_peripheral_clk_init(struct udevice *dev,
+					     struct cdns3_generic_peripheral
+					     *priv)
+{
+	int ret;
+
+	ret = clk_get_bulk(dev, &priv->clks);
+	if (ret)
+		return ret;
+
+#if CONFIG_IS_ENABLED(CLK)
+	ret = clk_enable_bulk(&priv->clks);
+	if (ret) {
+		clk_release_bulk(&priv->clks);
+		return ret;
+	}
+#endif
+
+	return 0;
+}
+
+int dm_usb_gadget_handle_interrupts(struct udevice *dev)
+{
+	struct cdns3_generic_peripheral *priv = dev_get_priv(dev);
+	struct cdns3 *cdns3 = &priv->cdns3;
+
+	cdns3_role_irq_handler(cdns3);
+
+	return 0;
+}
+
+static int cdns3_generic_peripheral_probe(struct udevice *dev)
+{
+	int ret;
+	struct cdns3_generic_peripheral *priv = dev_get_priv(dev);
+	struct cdns3 *cdns3 = &priv->cdns3;
+
+	cdns3->dev = dev;
+
+	ret = generic_phy_get_by_index(dev, 0, &priv->phy);
+	if (ret && ret != -ENOENT) {
+		printf("Failed to get USB PHY for %s\n", dev->name);
+		return ret;
+	}
+
+	ret = cdns3_generic_peripheral_clk_init(dev, priv);
+	if (ret)
+		return ret;
+
+	ret = cdns3_init(cdns3);
+
+	return 0;
+}
+
+static int cdns3_generic_peripheral_remove(struct udevice *dev)
+{
+	struct cdns3_generic_peripheral *priv = dev_get_priv(dev);
+	struct cdns3 *cdns3 = &priv->cdns3;
+
+	cdns3_exit(cdns3);
+
+	clk_release_bulk(&priv->clks);
+
+	if (generic_phy_valid(&priv->phy))
+		device_remove(priv->phy.dev, DM_REMOVE_NORMAL);
+
+	return 0;
+}
+
+static int cdns3_generic_peripheral_ofdata_to_platdata(struct udevice *dev)
+{
+	struct cdns3_generic_peripheral *priv = dev_get_priv(dev);
+	struct cdns3 *cdns3 = &priv->cdns3;
+
+	cdns3->none_core_regs = (void __iomem *)devfdt_get_addr_index(dev, 0);
+	cdns3->xhci_regs = (void __iomem *)devfdt_get_addr_index(dev, 1);
+	cdns3->dev_regs = (void __iomem *)devfdt_get_addr_index(dev, 2);
+	cdns3->phy_regs = (void __iomem *)devfdt_get_addr_index(dev, 3);
+	cdns3->otg_regs = (void __iomem *)devfdt_get_addr_index(dev, 4);
+
+	return 0;
+}
+
+static const struct udevice_id cdns3_generic_peripheral_ids[] = {
+	{ .compatible = "Cadence,usb3" },
+	{},
+};
+
+U_BOOT_DRIVER(cdns3_generic_peripheral) = {
+	.name	= "cdns3-generic-peripheral",
+	.id	= UCLASS_USB_GADGET_GENERIC,
+	.of_match = cdns3_generic_peripheral_ids,
+	.ofdata_to_platdata = cdns3_generic_peripheral_ofdata_to_platdata,
+	.probe = cdns3_generic_peripheral_probe,
+	.remove = cdns3_generic_peripheral_remove,
+	.priv_auto_alloc_size = sizeof(struct cdns3_generic_peripheral),
+};
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 0000000000..023f902ec9
--- /dev/null
+++ b/drivers/usb/cdns3/cdns3-nxp-reg-def.h
@@ -0,0 +1,89 @@ 
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2019 NXP
+ */
+
+#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	BIT(31)
+#define APB_SW_RESET	BIT(30)
+#define AXI_SW_RESET	BIT(29)
+#define RW_SW_RESET	BIT(28)
+#define PHY_SW_RESET	BIT(27)
+#define PHYAHB_SW_RESET	BIT(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	BIT(9)
+#define MDCTRL_CLK_SEL	BIT(7)
+#define MODE_STRAP_MASK	(0x7)
+#define DEV_MODE	BIT(2)
+#define HOST_MODE	BIT(1)
+#define OTG_MODE	BIT(0)
+
+/* USB3_INT_REG */
+#define CLK_125_REQ	BIT(29)
+#define LPM_CLK_REQ	BIT(28)
+#define DEVU3_WAEKUP_EN	BIT(14)
+#define OTG_WAKEUP_EN	BIT(12)
+#define DEV_INT_EN (3 << 8) /* DEV INT b9:8 */
+#define HOST_INT1_EN BIT(0) /* HOST INT b7:0 */
+
+/* USB3_CORE_STATUS */
+#define MDCTRL_CLK_STATUS	BIT(15)
+#define DEV_POWER_ON_READY	BIT(13)
+#define HOST_POWER_ON_READY	BIT(12)
+
+/* USB3_SSPHY_STATUS */
+#define PHY_REFCLK_REQ		BIT(0)
+
+/* PHY register definition */
+#define TB_ADDR_TX_RCVDETSC_CTRL	        (0x4124 * 4)
+
+/* Register bits definition */
+
+/* TB_ADDR_TX_RCVDETSC_CTRL */
+#define RXDET_IN_P3_32KHZ			BIT(0)
+
+/* OTG registers definition */
+#define OTGSTS		0x4
+#define OTGREFCLK	0xc
+
+/* Register bits definition */
+/* OTGSTS */
+#define OTG_NRDY	BIT(11)
+/* OTGREFCLK */
+#define OTG_STB_CLK_SWITCH_EN	BIT(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	BIT(25)
+
+/* XECP_AUX_CTRL_REG1 */
+#define CFG_RXDET_P3_EN		BIT(15)
+
+/* XECP_PM_PMCSR */
+#define PS_D0			BIT(0)
+#endif /* __DRIVERS_USB_CDNS3_NXP_H */
diff --git a/drivers/usb/cdns3/core.c b/drivers/usb/cdns3/core.c
new file mode 100644
index 0000000000..0f7573be10
--- /dev/null
+++ b/drivers/usb/cdns3/core.c
@@ -0,0 +1,197 @@ 
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2016 Cadence Design Systems - https://www.cadence.com/
+ * Copyright 2019 NXP
+ */
+#include <common.h>
+#include <malloc.h>
+#include <asm/dma-mapping.h>
+#include <asm/io.h>
+#include <linux/bug.h>
+#include <linux/list.h>
+#include <linux/compat.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/otg.h>
+
+#include "cdns3-nxp-reg-def.h"
+#include "core.h"
+#include "gadget-export.h"
+#include "gadget.h"
+
+static void cdns3_reset_core(struct cdns3 *cdns)
+{
+	setbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1, ALL_SW_RESET);
+	udelay(1);
+}
+
+static void cdns3_set_role(struct cdns3 *cdns, enum cdns3_roles role)
+{
+	u32 value;
+	int timeout_us = 100000;
+	struct cdns3_generic_peripheral *priv = container_of(cdns,
+			struct cdns3_generic_peripheral, cdns3);
+
+	if (role == CDNS3_ROLE_END)
+		return;
+
+	/* Wait clk value */
+	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
+	writel(value, cdns->none_core_regs + USB3_SSPHY_STATUS);
+	udelay(1);
+	value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
+	while ((value & 0xf0000000) != 0xf0000000 && timeout_us-- > 0) {
+		value = readl(cdns->none_core_regs + USB3_SSPHY_STATUS);
+		udelay(1);
+	}
+
+	if (timeout_us <= 0)
+		dev_err(cdns->dev, "wait clkvld timeout\n");
+
+	/* Set all Reset bits */
+	setbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1, ALL_SW_RESET);
+	udelay(1);
+
+	if (role == CDNS3_ROLE_HOST) {
+		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
+		value = (value & ~MODE_STRAP_MASK) | HOST_MODE | OC_DISABLE;
+		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
+		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
+			     PHYAHB_SW_RESET);
+		mdelay(1);
+		generic_phy_init(&priv->phy);
+		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
+			     RXDET_IN_P3_32KHZ);
+		udelay(10);
+		/* Force B Session Valid as 1 */
+		writel(0x0060, cdns->phy_regs + 0x380a4);
+		mdelay(1);
+
+		setbits_le32(cdns->none_core_regs + USB3_INT_REG, HOST_INT1_EN);
+
+		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
+			     ALL_SW_RESET);
+
+		dev_dbg(cdns->dev, "wait xhci_power_on_ready\n");
+
+		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & HOST_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(cdns->dev, "wait xhci_power_on_ready timeout\n");
+
+		dev_dbg(cdns->dev, "switch to host role successfully\n");
+	} else { /* gadget mode */
+		value = readl(cdns->none_core_regs + USB3_CORE_CTRL1);
+		value = (value & ~MODE_STRAP_MASK) | DEV_MODE;
+		writel(value, cdns->none_core_regs + USB3_CORE_CTRL1);
+		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
+			     PHYAHB_SW_RESET);
+
+		generic_phy_init(&priv->phy);
+		setbits_le32(cdns->phy_regs + TB_ADDR_TX_RCVDETSC_CTRL,
+			     RXDET_IN_P3_32KHZ);
+		udelay(10);
+		/* Force B Session Valid as 1 */
+		writel(0x0060, cdns->phy_regs + 0x380a4);
+		setbits_le32(cdns->none_core_regs + USB3_INT_REG, DEV_INT_EN);
+
+		clrbits_le32(cdns->none_core_regs + USB3_CORE_CTRL1,
+			     ALL_SW_RESET);
+
+		dev_dbg(cdns->dev, "wait gadget_power_on_ready\n");
+
+		value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
+		timeout_us = 100000;
+		while (!(value & DEV_POWER_ON_READY) && timeout_us-- > 0) {
+			value = readl(cdns->none_core_regs + USB3_CORE_STATUS);
+			udelay(1);
+		}
+
+		if (timeout_us <= 0)
+			dev_err(cdns->dev,
+				"wait gadget_power_on_ready timeout\n");
+
+		dev_dbg(cdns->dev, "switch to gadget role successfully\n");
+	}
+}
+
+static enum cdns3_roles cdns3_get_role(struct cdns3 *cdns)
+{
+	return cdns->roles[CDNS3_ROLE_HOST]
+		? CDNS3_ROLE_HOST
+		: CDNS3_ROLE_GADGET;
+}
+
+/**
+ * cdns3_core_init_role - initialize role of operation
+ * @cdns: Pointer to cdns3 structure
+ * @dr_mode: Role mode of device
+ *
+ * Returns 0 on success otherwise negative errno
+ */
+static int cdns3_core_init_role(struct cdns3 *cdns, enum usb_dr_mode dr_mode)
+{
+	cdns->role = CDNS3_ROLE_END;
+	if (dr_mode == USB_DR_MODE_UNKNOWN)
+		dr_mode = USB_DR_MODE_OTG;
+
+	/* Currently, only support gadget mode */
+	if (dr_mode == USB_DR_MODE_OTG || dr_mode == USB_DR_MODE_HOST) {
+		dev_err(cdns->dev, "doesn't support host and OTG, only for gadget\n");
+		return -EPERM;
+	}
+
+	if (dr_mode == USB_DR_MODE_PERIPHERAL) {
+		if (cdns3_gadget_init(cdns))
+			dev_info(cdns->dev, "doesn't support gadget\n");
+	}
+
+	if (!cdns->roles[CDNS3_ROLE_HOST] && !cdns->roles[CDNS3_ROLE_GADGET]) {
+		dev_err(cdns->dev, "no supported roles\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void cdns3_remove_roles(struct cdns3 *cdns)
+{
+	/* Only support gadget */
+	cdns3_gadget_remove(cdns);
+}
+
+int cdns3_init(struct cdns3 *cdns)
+{
+	int ret;
+
+	ret = cdns3_core_init_role(cdns, USB_DR_MODE_PERIPHERAL);
+
+	cdns->role = cdns3_get_role(cdns);
+	dev_dbg(dev, "the init role is %d\n", cdns->role);
+	cdns3_set_role(cdns, cdns->role);
+	ret = cdns3_role_start(cdns, cdns->role);
+	if (ret) {
+		dev_err(dev, "can't start %s role\n", cdns3_role(cdns)->name);
+		goto err;
+	}
+
+	dev_dbg(dev, "Cadence USB3 core: probe succeed\n");
+
+	return 0;
+
+err:
+	cdns3_remove_roles(cdns);
+
+	return ret;
+}
+
+void cdns3_exit(struct cdns3 *cdns)
+{
+	cdns3_role_stop(cdns);
+	cdns3_remove_roles(cdns);
+	cdns3_reset_core(cdns);
+}
diff --git a/drivers/usb/cdns3/core.h b/drivers/usb/cdns3/core.h
new file mode 100644
index 0000000000..d097678e52
--- /dev/null
+++ b/drivers/usb/cdns3/core.h
@@ -0,0 +1,118 @@ 
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright 2019 NXP
+ */
+
+#ifndef __DRIVERS_USB_CDNS3_CORE_H
+#define __DRIVERS_USB_CDNS3_CORE_H
+
+struct cdns3;
+enum cdns3_roles {
+	CDNS3_ROLE_HOST = 0,
+	CDNS3_ROLE_GADGET,
+	CDNS3_ROLE_END,
+};
+
+/**
+ * struct cdns3_role_driver - host/gadget role driver
+ * @start: start this role
+ * @stop: stop this role
+ * @suspend: suspend callback for this role
+ * @resume: resume callback for this role
+ * @irq: irq handler for this role
+ * @name: role name string (host/gadget)
+ */
+struct cdns3_role_driver {
+	int (*start)(struct cdns3 *cdns);
+	void (*stop)(struct cdns3 *cdns);
+	int (*suspend)(struct cdns3 *cdns, bool do_wakeup);
+	int (*resume)(struct cdns3 *cdns, bool hibernated);
+	int (*irq)(struct cdns3 *cdns);
+	const char *name;
+};
+
+#define CDNS3_NUM_OF_CLKS	5
+/**
+ * struct cdns3 - Representation of Cadence USB3 DRD controller.
+ * @dev: pointer to Cadence device struct
+ * @xhci_regs: pointer to base of xhci registers
+ * @xhci_res: the resource for xhci
+ * @dev_regs: pointer to base of dev registers
+ * @none_core_regs: pointer to base of nxp wrapper registers
+ * @phy_regs: pointer to base of phy registers
+ * @otg_regs: pointer to base of otg registers
+ * @irq: irq number for controller
+ * @roles: array of supported roles for this controller
+ * @role: current role
+ * @host_dev: the child host device pointer for cdns3 core
+ * @gadget_dev: the child gadget device pointer for cdns3 core
+ * @usbphy: usbphy for this controller
+ * @cdns3_clks: Clock pointer array for cdns3 core
+ * @extcon: Type-C extern connector
+ * @extcon_nb: notifier block for Type-C extern connector
+ * @role_switch_wq: work queue item for role switch
+ * @in_lpm: the controller in low power mode
+ * @wakeup_int: the wakeup interrupt
+ */
+struct cdns3 {
+	struct udevice *dev;
+	void __iomem *xhci_regs;
+	struct resource *xhci_res;
+	struct usbss_dev_register_block_type __iomem *dev_regs;
+	void __iomem *none_core_regs;
+	void __iomem *phy_regs;
+	void __iomem *otg_regs;
+	int irq;
+	struct cdns3_role_driver *roles[CDNS3_ROLE_END];
+	enum cdns3_roles role;
+	struct udevice *host_dev;
+	struct udevice *gadget_dev;
+	struct clk *cdns3_clks[CDNS3_NUM_OF_CLKS];
+
+	int  index;
+	struct list_head list;
+};
+
+static inline struct cdns3_role_driver *cdns3_role(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)
+{
+	if (role >= CDNS3_ROLE_END)
+		return 0;
+
+	if (!cdns->roles[role])
+		return -ENXIO;
+
+	cdns->role = role;
+	return cdns->roles[role]->start(cdns);
+}
+
+static inline void cdns3_role_stop(struct cdns3 *cdns)
+{
+	enum cdns3_roles role = cdns->role;
+
+	if (role == CDNS3_ROLE_END)
+		return;
+
+	cdns->roles[role]->stop(cdns);
+	cdns->role = CDNS3_ROLE_END;
+}
+
+static inline void cdns3_role_irq_handler(struct cdns3 *cdns)
+{
+	enum cdns3_roles role = cdns->role;
+
+	if (role == CDNS3_ROLE_END)
+		return;
+
+	cdns->roles[role]->irq(cdns);
+}
+
+int cdns3_init(struct cdns3 *cdns);
+void cdns3_exit(struct cdns3 *cdns);
+
+#endif /* __DRIVERS_USB_CDNS3_CORE_H */
diff --git a/drivers/usb/cdns3/dev-regs-macro.h b/drivers/usb/cdns3/dev-regs-macro.h
new file mode 100644
index 0000000000..85bf6c3772
--- /dev/null
+++ b/drivers/usb/cdns3/dev-regs-macro.h
@@ -0,0 +1,116 @@ 
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2016 Cadence Design Systems - http://www.cadence.com
+ * Copyright 2019 NXP
+ */
+
+#ifndef __REG_USBSS_DEV_ADDR_MAP_MACRO_H__
+#define __REG_USBSS_DEV_ADDR_MAP_MACRO_H__
+
+/* macros for field CFGRST */
+#define USB_CONF__CFGRST__MASK                                      0x00000001U
+#define USB_CONF__CFGSET__MASK                                      0x00000002U
+#define USB_CONF__USB3DIS__MASK                                     0x00000008U
+#define USB_CONF__DEVEN__MASK                                       0x00004000U
+#define USB_CONF__DEVDS__MASK                                       0x00008000U
+#define USB_CONF__L1EN__MASK                                        0x00010000U
+#define USB_CONF__L1DS__MASK                                        0x00020000U
+#define USB_CONF__CLK2OFFDS__MASK                                   0x00080000U
+#define USB_CONF__U1EN__MASK                                        0x01000000U
+#define USB_CONF__U1DS__MASK                                        0x02000000U
+#define USB_CONF__U2EN__MASK                                        0x04000000U
+#define USB_CONF__U2DS__MASK                                        0x08000000U
+
+/* macros for field CFGSTS */
+#define USB_STS__CFGSTS__MASK                                       0x00000001U
+#define USB_STS__USBSPEED__READ(src)     (((uint32_t)(src) & 0x00000070U) >> 4)
+
+/* macros for field ENDIAN_MIRROR */
+#define USB_STS__LPMST__READ(src)       (((uint32_t)(src) & 0x000c0000U) >> 18)
+
+/* macros for field USB2CONS */
+#define USB_STS__U1ENS__MASK                                        0x01000000U
+#define USB_STS__U2ENS__MASK                                        0x02000000U
+#define USB_STS__LST__READ(src)         (((uint32_t)(src) & 0x3c000000U) >> 26)
+
+/* macros for field SET_ADDR */
+#define USB_CMD__SET_ADDR__MASK                                     0x00000001U
+#define USB_CMD__STMODE						0x00000200U
+#define USB_CMD__TMODE_SEL(x)                                    (x << 10)
+#define USB_CMD__FADDR__WRITE(src)       (((uint32_t)(src) << 1) & 0x000000feU)
+
+/* macros for field CONIEN */
+#define USB_IEN__CONIEN__MASK                                       0x00000001U
+#define USB_IEN__DISIEN__MASK                                       0x00000002U
+#define USB_IEN__UWRESIEN__MASK                                     0x00000004U
+#define USB_IEN__UHRESIEN__MASK                                     0x00000008U
+#define USB_IEN__U3EXTIEN__MASK                                     0x00000020U
+#define USB_IEN__CON2IEN__MASK                                      0x00010000U
+#define USB_IEN__U2RESIEN__MASK                                     0x00040000U
+#define USB_IEN__L2ENTIEN__MASK                                     0x00100000U
+#define USB_IEN__L2EXTIEN__MASK                                     0x00200000U
+
+/* macros for field CONI */
+#define USB_ISTS__CONI__SHIFT                                                 0
+#define USB_ISTS__DISI__SHIFT                                                 1
+#define USB_ISTS__UWRESI__SHIFT                                               2
+#define USB_ISTS__UHRESI__SHIFT                                               3
+#define USB_ISTS__U3EXTI__SHIFT                                               5
+#define USB_ISTS__CON2I__SHIFT                                               16
+#define USB_ISTS__DIS2I__SHIFT                                               17
+#define USB_ISTS__DIS2I__MASK                                       0x00020000U
+#define USB_ISTS__U2RESI__SHIFT                                              18
+#define USB_ISTS__L2ENTI__SHIFT                                              20
+#define USB_ISTS__L2EXTI__SHIFT                                              21
+
+/* macros for field TRADDR */
+#define EP_TRADDR__TRADDR__WRITE(src)           ((uint32_t)(src) & 0xffffffffU)
+
+/* macros for field ENABLE */
+#define EP_CFG__ENABLE__MASK                                        0x00000001U
+#define EP_CFG__EPTYPE__WRITE(src)       (((uint32_t)(src) << 1) & 0x00000006U)
+#define EP_CFG__MAXBURST__WRITE(src)     (((uint32_t)(src) << 8) & 0x00000f00U)
+#define EP_CFG__MAXPKTSIZE__WRITE(src)  (((uint32_t)(src) << 16) & 0x07ff0000U)
+#define EP_CFG__BUFFERING__WRITE(src)   (((uint32_t)(src) << 27) & 0xf8000000U)
+
+/* macros for field EPRST */
+#define EP_CMD__EPRST__MASK                                         0x00000001U
+#define EP_CMD__SSTALL__MASK                                        0x00000002U
+#define EP_CMD__CSTALL__MASK                                        0x00000004U
+#define EP_CMD__ERDY__MASK                                          0x00000008U
+#define EP_CMD__REQ_CMPL__MASK                                      0x00000020U
+#define EP_CMD__DRDY__MASK                                          0x00000040U
+#define EP_CMD__DFLUSH__MASK                                        0x00000080U
+
+/* macros for field SETUP */
+#define EP_STS__SETUP__MASK                                         0x00000001U
+#define EP_STS__STALL__MASK                                         0x00000002U
+#define EP_STS__IOC__MASK                                           0x00000004U
+#define EP_STS__ISP__MASK                                           0x00000008U
+#define EP_STS__DESCMIS__MASK                                       0x00000010U
+#define EP_STS__TRBERR__MASK                                        0x00000080U
+#define EP_STS__NRDY__MASK                                          0x00000100U
+#define EP_STS__DBUSY__MASK                                         0x00000200U
+#define EP_STS__OUTSMM__MASK                                        0x00004000U
+#define EP_STS__ISOERR__MASK                                        0x00008000U
+
+/* macros for field SETUPEN */
+#define EP_STS_EN__SETUPEN__MASK                                    0x00000001U
+#define EP_STS_EN__DESCMISEN__MASK                                  0x00000010U
+#define EP_STS_EN__TRBERREN__MASK                                   0x00000080U
+
+/* macros for field EOUTEN0 */
+#define EP_IEN__EOUTEN0__MASK                                       0x00000001U
+#define EP_IEN__EINEN0__MASK                                        0x00010000U
+
+/* macros for field EOUT0 */
+#define EP_ISTS__EOUT0__MASK                                        0x00000001U
+#define EP_ISTS__EIN0__MASK                                         0x00010000U
+
+/* macros for field LFPS_MIN_DET_U1_EXIT */
+#define DBG_LINK1__LFPS_MIN_GEN_U1_EXIT__WRITE(src) \
+			(((uint32_t)(src)\
+			<< 8) & 0x0000ff00U)
+#define DBG_LINK1__LFPS_MIN_GEN_U1_EXIT_SET__MASK                   0x02000000U
+
+#endif /* __REG_USBSS_DEV_ADDR_MAP_MACRO_H__ */
diff --git a/drivers/usb/cdns3/dev-regs-map.h b/drivers/usb/cdns3/dev-regs-map.h
new file mode 100644
index 0000000000..c2d43068b7
--- /dev/null
+++ b/drivers/usb/cdns3/dev-regs-map.h
@@ -0,0 +1,117 @@ 
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2016 Cadence Design Systems - http://www.cadence.com
+ * Copyright 2019 NXP
+ */
+
+#ifndef __REG_USBSS_DEV_ADDR_MAP_H__
+#define __REG_USBSS_DEV_ADDR_MAP_H__
+
+#include "dev-regs-macro.h"
+
+struct usbss_dev_register_block_type {
+	u32 usb_conf;                     /*        0x0 - 0x4        */
+	u32 usb_sts;                      /*        0x4 - 0x8        */
+	u32 usb_cmd;                      /*        0x8 - 0xc        */
+	u32 usb_iptn;                     /*        0xc - 0x10       */
+	u32 usb_lpm;                      /*       0x10 - 0x14       */
+	u32 usb_ien;                      /*       0x14 - 0x18       */
+	u32 usb_ists;                     /*       0x18 - 0x1c       */
+	u32 ep_sel;                       /*       0x1c - 0x20       */
+	u32 ep_traddr;                    /*       0x20 - 0x24       */
+	u32 ep_cfg;                       /*       0x24 - 0x28       */
+	u32 ep_cmd;                       /*       0x28 - 0x2c       */
+	u32 ep_sts;                       /*       0x2c - 0x30       */
+	u32 ep_sts_sid;                   /*       0x30 - 0x34       */
+	u32 ep_sts_en;                    /*       0x34 - 0x38       */
+	u32 drbl;                         /*       0x38 - 0x3c       */
+	u32 ep_ien;                       /*       0x3c - 0x40       */
+	u32 ep_ists;                      /*       0x40 - 0x44       */
+	u32 usb_pwr;                      /*       0x44 - 0x48       */
+	u32 usb_conf2;                    /*       0x48 - 0x4c       */
+	u32 usb_cap1;                     /*       0x4c - 0x50       */
+	u32 usb_cap2;                     /*       0x50 - 0x54       */
+	u32 usb_cap3;                     /*       0x54 - 0x58       */
+	u32 usb_cap4;                     /*       0x58 - 0x5c       */
+	u32 usb_cap5;                     /*       0x5c - 0x60       */
+	u32 PAD2_73;                     /*       0x60 - 0x64       */
+	u32 usb_cpkt1;                    /*       0x64 - 0x68       */
+	u32 usb_cpkt2;                    /*       0x68 - 0x6c       */
+	u32 usb_cpkt3;                    /*       0x6c - 0x70       */
+	char pad__0[0x90];                     /*       0x70 - 0x100      */
+	u32 PAD2_78;                     /*      0x100 - 0x104      */
+	u32 dbg_link1;                    /*      0x104 - 0x108      */
+	u32 PAD2_80;                    /*      0x108 - 0x10c      */
+	u32 PAD2_81;                     /*      0x10c - 0x110      */
+	u32 PAD2_82;                     /*      0x110 - 0x114      */
+	u32 PAD2_83;                     /*      0x114 - 0x118      */
+	u32 PAD2_84;                     /*      0x118 - 0x11c      */
+	u32 PAD2_85;                     /*      0x11c - 0x120      */
+	u32 PAD2_86;                     /*      0x120 - 0x124      */
+	u32 PAD2_87;                    /*      0x124 - 0x128      */
+	u32 PAD2_88;                    /*      0x128 - 0x12c      */
+	u32 PAD2_89;                    /*      0x12c - 0x130      */
+	u32 PAD2_90;                    /*      0x130 - 0x134      */
+	u32 PAD2_91;                    /*      0x134 - 0x138      */
+	u32 PAD2_92;                    /*      0x138 - 0x13c      */
+	u32 PAD2_93;                    /*      0x13c - 0x140      */
+	u32 PAD2_94;                    /*      0x140 - 0x144      */
+	u32 PAD2_95;                    /*      0x144 - 0x148      */
+	u32 PAD2_96;                    /*      0x148 - 0x14c      */
+	u32 PAD2_97;                    /*      0x14c - 0x150      */
+	u32 PAD2_98;                    /*      0x150 - 0x154      */
+	u32 PAD2_99;                    /*      0x154 - 0x158      */
+	u32 PAD2_100;                    /*      0x158 - 0x15c      */
+	u32 PAD2_101;                    /*      0x15c - 0x160      */
+	u32 PAD2_102;                    /*      0x160 - 0x164      */
+	u32 PAD2_103;                    /*      0x164 - 0x168      */
+	u32 PAD2_104;                    /*      0x168 - 0x16c      */
+	u32 PAD2_105;                    /*      0x16c - 0x170      */
+	u32 PAD2_106;                    /*      0x170 - 0x174      */
+	u32 PAD2_107;                    /*      0x174 - 0x178      */
+	u32 PAD2_108;                    /*      0x178 - 0x17c      */
+	u32 PAD2_109;                    /*      0x17c - 0x180      */
+	u32 PAD2_110;                    /*      0x180 - 0x184      */
+	u32 PAD2_111;                    /*      0x184 - 0x188      */
+	u32 PAD2_112;                    /*      0x188 - 0x18c      */
+	char pad__1[0x20];                     /*      0x18c - 0x1ac      */
+	u32 PAD2_114;                    /*      0x1ac - 0x1b0      */
+	u32 PAD2_115;                    /*      0x1b0 - 0x1b4      */
+	u32 PAD2_116;                    /*      0x1b4 - 0x1b8      */
+	u32 PAD2_117;                    /*      0x1b8 - 0x1bc      */
+	u32 PAD2_118;                    /*      0x1bc - 0x1c0      */
+	u32 PAD2_119;                    /*      0x1c0 - 0x1c4      */
+	u32 PAD2_120;                    /*      0x1c4 - 0x1c8      */
+	u32 PAD2_121;                    /*      0x1c8 - 0x1cc      */
+	u32 PAD2_122;                    /*      0x1cc - 0x1d0      */
+	u32 PAD2_123;                    /*      0x1d0 - 0x1d4      */
+	u32 PAD2_124;                    /*      0x1d4 - 0x1d8      */
+	u32 PAD2_125;                    /*      0x1d8 - 0x1dc      */
+	u32 PAD2_126;                    /*      0x1dc - 0x1e0      */
+	u32 PAD2_127;                    /*      0x1e0 - 0x1e4      */
+	u32 PAD2_128;                    /*      0x1e4 - 0x1e8      */
+	u32 PAD2_129;                    /*      0x1e8 - 0x1ec      */
+	u32 PAD2_130;                    /*      0x1ec - 0x1f0      */
+	u32 PAD2_131;                    /*      0x1f0 - 0x1f4      */
+	u32 PAD2_132;                    /*      0x1f4 - 0x1f8      */
+	u32 PAD2_133;                    /*      0x1f8 - 0x1fc      */
+	u32 PAD2_134;                    /*      0x1fc - 0x200      */
+	u32 PAD2_135;                    /*      0x200 - 0x204      */
+	u32 PAD2_136;                    /*      0x204 - 0x208      */
+	u32 PAD2_137;                    /*      0x208 - 0x20c      */
+	u32 PAD2_138;                    /*      0x20c - 0x210      */
+	u32 PAD2_139;                    /*      0x210 - 0x214      */
+	u32 PAD2_140;                    /*      0x214 - 0x218      */
+	u32 PAD2_141;                    /*      0x218 - 0x21c      */
+	u32 PAD2_142;                    /*      0x21c - 0x220      */
+	u32 PAD2_143;                    /*      0x220 - 0x224      */
+	u32 PAD2_144;                    /*      0x224 - 0x228      */
+	char pad__2[0xd8];                     /*      0x228 - 0x300      */
+	u32 dma_axi_ctrl;                 /*      0x300 - 0x304      */
+	u32 PAD2_147;                   /*      0x304 - 0x308      */
+	u32 PAD2_148;                  /*      0x308 - 0x30c      */
+	u32 PAD2_149;                /*      0x30c - 0x310      */
+	u32 PAD2_150;                /*      0x310 - 0x314      */
+};
+
+#endif /* __REG_USBSS_DEV_ADDR_MAP_H__ */
diff --git a/drivers/usb/cdns3/gadget-export.h b/drivers/usb/cdns3/gadget-export.h
new file mode 100644
index 0000000000..0b011b56c6
--- /dev/null
+++ b/drivers/usb/cdns3/gadget-export.h
@@ -0,0 +1,26 @@ 
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2019 NXP
+ */
+
+#ifndef __CDNS3_GADGET_EXPORT_H
+#define __CDNS3_GADGET_EXPORT_H
+
+#ifdef CONFIG_USB_CDNS3_GADGET
+
+int cdns3_gadget_init(struct cdns3 *cdns);
+void cdns3_gadget_remove(struct cdns3 *cdns);
+#else
+
+static inline int cdns3_gadget_init(struct cdns3 *cdns)
+{
+	return -ENXIO;
+}
+
+static inline void cdns3_gadget_remove(struct cdns3 *cdns)
+{
+}
+
+#endif
+
+#endif /* __CDNS3_GADGET_EXPORT_H */
diff --git a/drivers/usb/cdns3/gadget.c b/drivers/usb/cdns3/gadget.c
new file mode 100644
index 0000000000..c5a02d9f4e
--- /dev/null
+++ b/drivers/usb/cdns3/gadget.c
@@ -0,0 +1,2222 @@ 
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (C) 2016 Cadence Design Systems - https://www.cadence.com/
+ * Copyright 2019 NXP
+ */
+#include <common.h>
+#include <malloc.h>
+#include <asm/dma-mapping.h>
+#include <asm/io.h>
+#include <linux/bug.h>
+#include <linux/list.h>
+#include <linux/compat.h>
+
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/composite.h>
+
+#include "core.h"
+#include "gadget-export.h"
+#include "gadget.h"
+#include "io.h"
+
+static void __cdns3_gadget_start(struct usb_ss_dev *usb_ss);
+static void cdns_prepare_setup_packet(struct usb_ss_dev *usb_ss);
+static void cdns_ep_config(struct usb_ss_endpoint *usb_ss_ep);
+
+static const char *const speed_names[] = {
+	[USB_SPEED_UNKNOWN] = "UNKNOWN",
+	[USB_SPEED_LOW] = "low-speed",
+	[USB_SPEED_FULL] = "full-speed",
+	[USB_SPEED_HIGH] = "high-speed",
+	[USB_SPEED_WIRELESS] = "wireless",
+	[USB_SPEED_SUPER] = "super-speed",
+};
+
+const char *usb_speed_string(enum usb_device_speed speed)
+{
+	if (speed < 0 || speed >= ARRAY_SIZE(speed_names))
+		speed = USB_SPEED_UNKNOWN;
+	return speed_names[speed];
+}
+
+static struct usb_endpoint_descriptor cdns3_gadget_ep0_desc = {
+	.bLength	= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType = USB_DT_ENDPOINT,
+	.bmAttributes	= USB_ENDPOINT_XFER_CONTROL,
+};
+
+/**
+ * next_request - returns next request from list
+ * @list: list containing requests
+ *
+ * Returns request or NULL if no requests in list
+ */
+static struct usb_request *next_request(struct list_head *list)
+{
+	if (list_empty(list))
+		return NULL;
+	return list_first_entry(list, struct usb_request, list);
+}
+
+/**
+ * select_ep - selects endpoint
+ * @usb_ss: extended gadget object
+ * @ep: endpoint address
+ */
+static void select_ep(struct usb_ss_dev *usb_ss, u32 ep)
+{
+	if (!usb_ss || !usb_ss->regs) {
+		dev_err(&usb_ss->dev, "Failed to select endpoint!\n");
+		return;
+	}
+
+	cdns_writel(&usb_ss->regs->ep_sel, ep);
+}
+
+/**
+ * usb_ss_allocate_trb_pool - Allocates TRB's pool for selected endpoint
+ * @usb_ss_ep: extended endpoint object
+ *
+ * Function will return 0 on success or -ENOMEM on allocation error
+ */
+static int usb_ss_allocate_trb_pool(struct usb_ss_endpoint *usb_ss_ep)
+{
+	if (usb_ss_ep->trb_pool)
+		return 0;
+
+	usb_ss_ep->trb_pool =
+		dma_alloc_coherent(sizeof(struct usb_ss_trb) * USB_SS_TRBS_NUM,
+				   (unsigned long *)&usb_ss_ep->trb_pool_dma);
+
+	if (!usb_ss_ep->trb_pool) {
+		dev_err(&usb_ss_ep->usb_ss->dev,
+			"Failed to allocate TRB pool for endpoint %s\n",
+			usb_ss_ep->name);
+		return -ENOMEM;
+	}
+
+	memset(usb_ss_ep->trb_pool, 0,
+	       sizeof(struct usb_ss_trb) * USB_SS_TRBS_NUM);
+
+	return 0;
+}
+
+/**
+ * cdns_ep_stall_flush - Stalls and flushes selected endpoint
+ * @usb_ss_ep: extended endpoint object
+ *
+ * Endpoint must be selected before call to this function
+ */
+static void cdns_ep_stall_flush(struct usb_ss_endpoint *usb_ss_ep)
+{
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__DFLUSH__MASK | EP_CMD__ERDY__MASK |
+		      EP_CMD__SSTALL__MASK);
+
+	/* wait for DFLUSH cleared */
+	while (cdns_readl(&usb_ss->regs->ep_cmd) &
+	       EP_CMD__DFLUSH__MASK)
+		;
+
+	usb_ss_ep->stalled_flag = 1;
+}
+
+/**
+ * cdns_ep0_config - Configures default endpoint
+ * @usb_ss: extended gadget object
+ *
+ * Functions sets parameters: maximal packet size and enables interrupts
+ */
+static void cdns_ep0_config(struct usb_ss_dev *usb_ss)
+{
+	u32 max_packet_size = 0;
+
+	switch (usb_ss->gadget.speed) {
+	case USB_SPEED_UNKNOWN:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_0;
+		usb_ss->gadget.ep0->maxpacket = ENDPOINT_MAX_PACKET_SIZE_0;
+		cdns3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(0);
+		break;
+
+	case USB_SPEED_LOW:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_8;
+		usb_ss->gadget.ep0->maxpacket = ENDPOINT_MAX_PACKET_SIZE_8;
+		cdns3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(8);
+		break;
+
+	case USB_SPEED_FULL:
+	case USB_SPEED_HIGH:
+	case USB_SPEED_WIRELESS:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_64;
+		usb_ss->gadget.ep0->maxpacket = ENDPOINT_MAX_PACKET_SIZE_64;
+		cdns3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(64);
+		break;
+
+	case USB_SPEED_SUPER:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_512;
+		usb_ss->gadget.ep0->maxpacket = ENDPOINT_MAX_PACKET_SIZE_512;
+		cdns3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
+		break;
+	}
+
+	/* init ep out */
+	select_ep(usb_ss, USB_DIR_OUT);
+
+	cdns_writel(&usb_ss->regs->ep_cfg,
+		      EP_CFG__ENABLE__MASK |
+		      EP_CFG__MAXPKTSIZE__WRITE(max_packet_size));
+	cdns_writel(&usb_ss->regs->ep_sts_en,
+		      EP_STS_EN__SETUPEN__MASK |
+		      EP_STS_EN__DESCMISEN__MASK |
+		      EP_STS_EN__TRBERREN__MASK);
+
+	/* init ep in */
+	select_ep(usb_ss, USB_DIR_IN);
+
+	cdns_writel(&usb_ss->regs->ep_cfg,
+		      EP_CFG__ENABLE__MASK |
+		      EP_CFG__MAXPKTSIZE__WRITE(max_packet_size));
+	cdns_writel(&usb_ss->regs->ep_sts_en,
+		      EP_STS_EN__SETUPEN__MASK |
+		      EP_STS_EN__TRBERREN__MASK);
+
+	cdns_prepare_setup_packet(usb_ss);
+}
+
+/**
+ * cdns_gadget_unconfig - Unconfigures device controller
+ * @usb_ss: extended gadget object
+ */
+static void cdns_gadget_unconfig(struct usb_ss_dev *usb_ss)
+{
+	/* RESET CONFIGURATION */
+	cdns_writel(&usb_ss->regs->usb_conf,
+		      USB_CONF__CFGRST__MASK);
+
+	usb_ss->hw_configured_flag = 0;
+}
+
+/**
+ * cdns_ep0_run_transfer - Do transfer on default endpoint hardware
+ * @usb_ss: extended gadget object
+ * @dma_addr: physical address where data is/will be stored
+ * @length: data length
+ * @erdy: set it to 1 when ERDY packet should be sent -
+ *        exit from flow control state
+ */
+static void cdns_ep0_run_transfer(struct usb_ss_dev *usb_ss,
+				  dma_addr_t dma_addr,
+				  unsigned int length, int erdy)
+{
+	usb_ss->trb_ep0[0] = TRB_SET_DATA_BUFFER_POINTER(dma_addr);
+	usb_ss->trb_ep0[1] = TRB_SET_TRANSFER_LENGTH((u32)length);
+	usb_ss->trb_ep0[2] = TRB_SET_CYCLE_BIT |
+		TRB_SET_INT_ON_COMPLETION | TRB_TYPE_NORMAL;
+
+	cdns_flush_cache((uintptr_t)usb_ss->trb_ep0, 20);
+	cdns_flush_cache((uintptr_t)dma_addr, length);
+
+	dev_dbg(&usb_ss->dev, "DRBL(%02X)\n",
+		usb_ss->ep0_data_dir ? USB_DIR_IN : USB_DIR_OUT);
+
+	select_ep(usb_ss, usb_ss->ep0_data_dir
+		? USB_DIR_IN : USB_DIR_OUT);
+
+	cdns_writel(&usb_ss->regs->ep_traddr,
+		      EP_TRADDR__TRADDR__WRITE(usb_ss->trb_ep0_dma));
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__DRDY__MASK); /* drbl */
+
+	if (erdy)
+		cdns_writel(&usb_ss->regs->ep_cmd,
+			      EP_CMD__ERDY__MASK);
+}
+
+/**
+ * cdns_ep_run_transfer - Do transfer on no-default endpoint hardware
+ * @usb_ss_ep: extended endpoint object
+ *
+ * Returns zero on success or negative value on failure
+ */
+static int cdns_ep_run_transfer(struct usb_ss_endpoint *usb_ss_ep)
+{
+	dma_addr_t trb_dma;
+	struct usb_request *request = next_request(&usb_ss_ep->request_list);
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	struct usb_ss_trb *trb;
+
+	if (!request)
+		return -EINVAL;
+
+	dev_dbg(&usb_ss->dev, "DRBL(%02X)\n",
+		usb_ss_ep->endpoint.desc->bEndpointAddress);
+
+	usb_ss_ep->hw_pending_flag = 1;
+	trb_dma = request->dma;
+
+	/* must allocate buffer aligned to 8 */
+	if (request->dma % ADDR_MODULO_8) {
+		memcpy(usb_ss_ep->cpu_addr, request->buf, request->length);
+		trb_dma = usb_ss_ep->dma_addr;
+	}
+
+	cdns_flush_cache((uintptr_t)trb_dma, request->length);
+
+	trb = usb_ss_ep->trb_pool;
+
+	/* fill TRB */
+	trb->offset0 = trb_dma;
+
+	trb->offset4 = TRB_SET_BURST_LENGTH(16) |
+		TRB_SET_TRANSFER_LENGTH(request->length);
+
+	trb->offset8 = TRB_SET_CYCLE_BIT
+		| TRB_SET_INT_ON_COMPLETION
+		| TRB_SET_INT_ON_SHORT_PACKET
+		| TRB_TYPE_NORMAL;
+
+	cdns_flush_cache((uintptr_t)trb, sizeof(struct usb_ss_trb));
+
+	/* arm transfer on selected endpoint */
+	select_ep(usb_ss_ep->usb_ss,
+		  usb_ss_ep->endpoint.desc->bEndpointAddress);
+
+	cdns_writel(&usb_ss->regs->ep_traddr,
+		      EP_TRADDR__TRADDR__WRITE(usb_ss_ep->trb_pool_dma));
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__DRDY__MASK); /* DRDY */
+	return 0;
+}
+
+/**
+ * cdns_get_setup_ret - Returns status of handling setup packet
+ * Setup is handled by gadget driver
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns zero on success or negative value on failure
+ */
+static int cdns_get_setup_ret(struct usb_ss_dev *usb_ss,
+			      struct usb_ctrlrequest *ctrl_req)
+{
+	int ret;
+
+	spin_unlock(&usb_ss->lock);
+	usb_ss->setup_pending = 1;
+	ret = usb_ss->gadget_driver->setup(&usb_ss->gadget, ctrl_req);
+	usb_ss->setup_pending = 0;
+	spin_lock(&usb_ss->lock);
+	return ret;
+}
+
+static void cdns_prepare_setup_packet(struct usb_ss_dev *usb_ss)
+{
+	usb_ss->ep0_data_dir = 0;
+	cdns_ep0_run_transfer(usb_ss, usb_ss->setup_dma, 8, 0);
+}
+
+/**
+ * cdns_req_ep0_set_address - Handling of SET_ADDRESS standard USB request
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_req_ep0_set_address(struct usb_ss_dev *usb_ss,
+				    struct usb_ctrlrequest *ctrl_req)
+{
+	enum usb_device_state device_state = usb_ss->gadget.state;
+	u32 reg;
+	u32 addr;
+
+	addr = le16_to_cpu(ctrl_req->wValue);
+
+	if (addr > DEVICE_ADDRESS_MAX) {
+		dev_err(&usb_ss->dev,
+			"Device address (%d) cannot be greater than %d\n",
+				addr, DEVICE_ADDRESS_MAX);
+		return -EINVAL;
+	}
+
+	if (device_state == USB_STATE_CONFIGURED) {
+		dev_err(&usb_ss->dev, "USB device already configured\n");
+		return -EINVAL;
+	}
+
+	reg = cdns_readl(&usb_ss->regs->usb_cmd);
+
+	cdns_writel(&usb_ss->regs->usb_cmd, reg
+			| USB_CMD__FADDR__WRITE(addr)
+			| USB_CMD__SET_ADDR__MASK);
+
+	usb_gadget_set_state(&usb_ss->gadget,
+			     (addr ? USB_STATE_ADDRESS : USB_STATE_DEFAULT));
+
+	cdns_prepare_setup_packet(usb_ss);
+
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__ERDY__MASK | EP_CMD__REQ_CMPL__MASK);
+	return 0;
+}
+
+/**
+ * cdns_req_ep0_get_status - Handling of GET_STATUS standard USB request
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_req_ep0_get_status(struct usb_ss_dev *usb_ss,
+				   struct usb_ctrlrequest *ctrl_req)
+{
+	u16 usb_status = 0;
+	unsigned int length = 2;
+	u32 recip = ctrl_req->bRequestType & USB_RECIP_MASK;
+	u32 reg;
+
+	switch (recip) {
+	case USB_RECIP_DEVICE:
+		reg = cdns_readl(&usb_ss->regs->usb_sts);
+
+		if (reg & USB_STS__U1ENS__MASK)
+			usb_status |= 1uL << USB_DEV_STAT_U1_ENABLED;
+
+		if (reg & USB_STS__U2ENS__MASK)
+			usb_status |= 1uL << USB_DEV_STAT_U2_ENABLED;
+
+		if (usb_ss->wake_up_flag)
+			usb_status |= 1uL << USB_DEVICE_REMOTE_WAKEUP;
+
+		/* self powered */
+		usb_status |= 1uL << USB_DEVICE_SELF_POWERED;
+		break;
+
+	case USB_RECIP_INTERFACE:
+		return cdns_get_setup_ret(usb_ss, ctrl_req);
+
+	case USB_RECIP_ENDPOINT:
+		/* check if endpoint is stalled */
+		select_ep(usb_ss, ctrl_req->wIndex);
+		if (cdns_readl(&usb_ss->regs->ep_sts)
+			& EP_STS__STALL__MASK)
+			usb_status = 1;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	*(u16 *)usb_ss->setup = cpu_to_le16(usb_status);
+
+	usb_ss->actual_ep0_request = NULL;
+	cdns_ep0_run_transfer(usb_ss, usb_ss->setup_dma, length, 1);
+	return 0;
+}
+
+/**
+ * cdns_req_ep0_handle_feature -
+ * Handling of GET/SET_FEATURE standard USB request
+ *
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ * @set: must be set to 1 for SET_FEATURE request
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_req_ep0_handle_feature(struct usb_ss_dev *usb_ss,
+				       struct usb_ctrlrequest *ctrl_req,
+				       int set)
+{
+	u32 recip = ctrl_req->bRequestType & USB_RECIP_MASK;
+	struct usb_ss_endpoint *usb_ss_ep;
+	u32 reg;
+	u8 tmode = 0;
+
+	switch (recip) {
+	case USB_RECIP_DEVICE:
+
+		switch (ctrl_req->wValue) {
+		case USB_DEVICE_U1_ENABLE:
+			if (usb_ss->gadget.state != USB_STATE_CONFIGURED)
+				return -EINVAL;
+			if (usb_ss->gadget.speed != USB_SPEED_SUPER)
+				return -EINVAL;
+
+			reg = cdns_readl(&usb_ss->regs->usb_conf);
+			if (set)
+				/* set U1EN */
+				reg |= USB_CONF__U1EN__MASK;
+			else
+				/* set U1 disable */
+				reg |= USB_CONF__U1DS__MASK;
+			cdns_writel(&usb_ss->regs->usb_conf, reg);
+			break;
+
+		case USB_DEVICE_U2_ENABLE:
+			if (usb_ss->gadget.state != USB_STATE_CONFIGURED)
+				return -EINVAL;
+			if (usb_ss->gadget.speed != USB_SPEED_SUPER)
+				return -EINVAL;
+
+			reg = cdns_readl(&usb_ss->regs->usb_conf);
+			if (set)
+				/* set U2EN */
+				reg |= USB_CONF__U2EN__MASK;
+			else
+				/* set U2 disable */
+				reg |= USB_CONF__U2DS__MASK;
+			cdns_writel(&usb_ss->regs->usb_conf, reg);
+			break;
+
+		case USB_DEVICE_A_ALT_HNP_SUPPORT:
+			break;
+
+		case USB_DEVICE_A_HNP_SUPPORT:
+			break;
+
+		case USB_DEVICE_B_HNP_ENABLE:
+			if (!usb_ss->gadget.b_hnp_enable && set)
+				usb_ss->gadget.b_hnp_enable = 1;
+			break;
+
+		case USB_DEVICE_REMOTE_WAKEUP:
+			usb_ss->wake_up_flag = !!set;
+			break;
+
+		case USB_DEVICE_TEST_MODE:
+			if (usb_ss->gadget.state != USB_STATE_CONFIGURED)
+				return -EINVAL;
+			if (usb_ss->gadget.speed != USB_SPEED_HIGH &&
+			    usb_ss->gadget.speed != USB_SPEED_FULL)
+				return -EINVAL;
+			if (ctrl_req->wLength != 0 ||
+			    ctrl_req->bRequestType & USB_DIR_IN) {
+				dev_err(&usb_ss->dev, "req is error\n");
+				return -EINVAL;
+			}
+			tmode = le16_to_cpu(ctrl_req->wIndex) >> 8;
+			switch (tmode) {
+			case TEST_J:
+			case TEST_K:
+			case TEST_SE0_NAK:
+			case TEST_PACKET:
+				reg = cdns_readl(&usb_ss->regs->usb_cmd);
+				tmode -= 1;
+				reg |= USB_CMD__STMODE |
+					USB_CMD__TMODE_SEL(tmode);
+				cdns_writel(&usb_ss->regs->usb_cmd, reg);
+				dev_info(&usb_ss->dev,
+					 "set test mode, val=0x%x", reg);
+				break;
+			default:
+				return -EINVAL;
+			}
+			break;
+
+		default:
+			return -EINVAL;
+		}
+		break;
+
+	case USB_RECIP_INTERFACE:
+		return cdns_get_setup_ret(usb_ss, ctrl_req);
+
+	case USB_RECIP_ENDPOINT:
+		select_ep(usb_ss, ctrl_req->wIndex);
+		u8 ep_index = CAST_EP_ADDR_TO_INDEX(ctrl_req->wIndex);
+
+		if (set) {
+			/* set stall */
+			cdns_writel(&usb_ss->regs->ep_cmd,
+				      EP_CMD__SSTALL__MASK);
+
+			/* handle non zero endpoint software endpoint */
+			if (ctrl_req->wIndex & 0x7F) {
+				usb_ss_ep = usb_ss->eps[ep_index];
+				usb_ss_ep->stalled_flag = 1;
+			}
+		} else {
+			struct usb_request *request;
+
+			if (ctrl_req->wIndex & 0x7F) {
+				if (usb_ss->eps[ep_index]->wedge_flag)
+					goto jmp_wedge;
+			}
+
+			/* clear stall */
+			cdns_writel(&usb_ss->regs->ep_cmd,
+				      EP_CMD__CSTALL__MASK |
+				      EP_CMD__EPRST__MASK);
+			/* wait for EPRST cleared */
+			while (cdns_readl(&usb_ss->regs->ep_cmd)
+					& EP_CMD__EPRST__MASK)
+				;
+
+			/* handle non zero endpoint software endpoint */
+			if (ctrl_req->wIndex & 0x7F) {
+				usb_ss_ep = usb_ss->eps[ep_index];
+				usb_ss_ep->stalled_flag = 0;
+
+				request =
+				    next_request(&usb_ss_ep->request_list);
+				if (request)
+					cdns_ep_run_transfer(usb_ss_ep);
+			}
+		}
+jmp_wedge:
+		select_ep(usb_ss, 0x00);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__ERDY__MASK | EP_CMD__REQ_CMPL__MASK);
+
+	return 0;
+}
+
+/**
+ * cdns_req_ep0_set_sel - Handling of SET_SEL standard USB request
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_req_ep0_set_sel(struct usb_ss_dev *usb_ss,
+				struct usb_ctrlrequest *ctrl_req)
+{
+	if (usb_ss->gadget.state < USB_STATE_ADDRESS)
+		return -EINVAL;
+
+	if (ctrl_req->wLength != 6) {
+		dev_err(&usb_ss->dev, "Set SEL should be 6 bytes, got %d\n",
+			ctrl_req->wLength);
+		return -EINVAL;
+	}
+
+	usb_ss->ep0_data_dir = 0;
+	usb_ss->actual_ep0_request = NULL;
+	cdns_ep0_run_transfer(usb_ss, usb_ss->setup_dma, 6, 1);
+
+	return 0;
+}
+
+/**
+ * cdns_req_ep0_set_isoch_delay -
+ * Handling of GET_ISOCH_DELAY standard USB request
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_req_ep0_set_isoch_delay(struct usb_ss_dev *usb_ss,
+					struct usb_ctrlrequest *ctrl_req)
+{
+	if (ctrl_req->wIndex || ctrl_req->wLength)
+		return -EINVAL;
+
+	usb_ss->isoch_delay = ctrl_req->wValue;
+	cdns_writel(&usb_ss->regs->ep_cmd,
+		      EP_CMD__ERDY__MASK | EP_CMD__REQ_CMPL__MASK);
+	return 0;
+}
+
+/**
+ * cdns_req_ep0_set_configuration - Handling of SET_CONFIG standard USB request
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, 0x7FFF on deferred status stage, error code on error
+ */
+static int cdns_req_ep0_set_configuration(struct usb_ss_dev *usb_ss,
+					  struct usb_ctrlrequest *ctrl_req)
+{
+	enum usb_device_state device_state = usb_ss->gadget.state;
+	u32 config = le16_to_cpu(ctrl_req->wValue);
+	struct usb_ep *ep;
+	struct usb_ss_endpoint *usb_ss_ep, *temp_ss_ep;
+	int i, result = 0;
+
+	switch (device_state) {
+	case USB_STATE_ADDRESS:
+		/* Configure non-control EPs */
+		list_for_each_entry_safe(usb_ss_ep, temp_ss_ep,
+					 &usb_ss->ep_match_list,
+					 ep_match_pending_list) {
+			cdns_ep_config(usb_ss_ep);
+			list_del(&usb_ss_ep->ep_match_pending_list);
+		}
+
+		list_for_each_entry(ep, &usb_ss->gadget.ep_list, ep_list) {
+			usb_ss_ep = to_usb_ss_ep(ep);
+			if (usb_ss_ep->used)
+				cdns_ep_config(usb_ss_ep);
+		}
+
+#ifdef CDNS_THREADED_IRQ_HANDLING
+		usb_ss->ep_ien = cdns_readl(&usb_ss->regs->ep_ien)
+			| EP_IEN__EOUTEN0__MASK | EP_IEN__EINEN0__MASK;
+#endif
+		result = cdns_get_setup_ret(usb_ss, ctrl_req);
+
+		if (result != 0)
+			return result;
+
+		if (config) {
+			if (!usb_ss->hw_configured_flag) {
+				/* SET CONFIGURATION */
+				cdns_writel(&usb_ss->regs->usb_conf,
+					      USB_CONF__CFGSET__MASK);
+				cdns_writel(&usb_ss->regs->ep_cmd,
+					      EP_CMD__ERDY__MASK |
+					      EP_CMD__REQ_CMPL__MASK);
+				/* wait until configuration set */
+				while (!(cdns_readl(&usb_ss->regs->usb_sts)
+				       & USB_STS__CFGSTS__MASK))
+					;
+				usb_ss->hw_configured_flag = 1;
+
+				list_for_each_entry(ep, &usb_ss->gadget.ep_list,
+						    ep_list) {
+					usb_ss_ep = to_usb_ss_ep(ep);
+					if (usb_ss_ep->enabled)
+						cdns_ep_run_transfer(usb_ss_ep);
+				}
+			}
+
+			usb_gadget_set_state(&usb_ss->gadget,
+					     USB_STATE_CONFIGURED);
+
+		} else {
+			cdns_gadget_unconfig(usb_ss);
+			for (i = 0; i < usb_ss->ep_nums; i++)
+				usb_ss->eps[i]->enabled = 0;
+			usb_gadget_set_state(&usb_ss->gadget,
+					     USB_STATE_ADDRESS);
+		}
+		break;
+
+	case USB_STATE_CONFIGURED:
+		result = cdns_get_setup_ret(usb_ss, ctrl_req);
+		if (!config && !result) {
+			cdns_gadget_unconfig(usb_ss);
+			for (i = 0; i < usb_ss->ep_nums; i++)
+				usb_ss->eps[i]->enabled = 0;
+			usb_gadget_set_state(&usb_ss->gadget,
+					     USB_STATE_ADDRESS);
+		}
+		break;
+
+	default:
+		result = -EINVAL;
+	}
+
+	return result;
+}
+
+/**
+ * cdns_ep0_standard_request - Handling standard USB requests
+ * @usb_ss: extended gadget object
+ * @ctrl_req: pointer to received setup packet
+ *
+ * Returns 0 if success, error code on error
+ */
+static int cdns_ep0_standard_request(struct usb_ss_dev *usb_ss,
+				     struct usb_ctrlrequest *ctrl_req)
+{
+	switch (ctrl_req->bRequest) {
+	case USB_REQ_SET_ADDRESS:
+		return cdns_req_ep0_set_address(usb_ss, ctrl_req);
+	case USB_REQ_SET_CONFIGURATION:
+		return cdns_req_ep0_set_configuration(usb_ss, ctrl_req);
+	case USB_REQ_GET_STATUS:
+		return cdns_req_ep0_get_status(usb_ss, ctrl_req);
+	case USB_REQ_CLEAR_FEATURE:
+		return cdns_req_ep0_handle_feature(usb_ss, ctrl_req, 0);
+	case USB_REQ_SET_FEATURE:
+		return cdns_req_ep0_handle_feature(usb_ss, ctrl_req, 1);
+	case USB_REQ_SET_SEL:
+		return cdns_req_ep0_set_sel(usb_ss, ctrl_req);
+	case USB_REQ_SET_ISOCH_DELAY:
+		return cdns_req_ep0_set_isoch_delay(usb_ss, ctrl_req);
+	default:
+		return cdns_get_setup_ret(usb_ss, ctrl_req);
+	}
+}
+
+/**
+ * cdns_ep0_setup_phase - Handling setup USB requests
+ * @usb_ss: extended gadget object
+ */
+static void cdns_ep0_setup_phase(struct usb_ss_dev *usb_ss)
+{
+	int result;
+	struct usb_ctrlrequest *ctrl_req =
+			(struct usb_ctrlrequest *)usb_ss->setup;
+
+	if ((ctrl_req->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD)
+		result = cdns_ep0_standard_request(usb_ss, ctrl_req);
+	else
+		result = cdns_get_setup_ret(usb_ss, ctrl_req);
+
+	if (result != 0 && result != USB_GADGET_DELAYED_STATUS) {
+		dev_dbg(&usb_ss->dev, "STALL(00) %d\n", result);
+
+		/* set_stall on ep0 */
+		select_ep(usb_ss, 0x00);
+		cdns_writel(&usb_ss->regs->ep_cmd,
+			      EP_CMD__SSTALL__MASK);
+		cdns_writel(&usb_ss->regs->ep_cmd,
+			      EP_CMD__ERDY__MASK | EP_CMD__REQ_CMPL__MASK);
+		return;
+	}
+}
+
+/**
+ * cdns_check_ep_interrupt_proceed - Processes interrupt related to endpoint
+ * @usb_ss_ep: extended endpoint object
+ *
+ * Returns 0
+ */
+static int cdns_check_ep_interrupt_proceed(struct usb_ss_endpoint *usb_ss_ep)
+{
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	struct usb_request *request;
+	u32 ep_sts_reg;
+
+	select_ep(usb_ss, usb_ss_ep->address);
+	ep_sts_reg = cdns_readl(&usb_ss->regs->ep_sts);
+
+	dev_dbg(&usb_ss->dev, "EP_STS: %08X\n", ep_sts_reg);
+
+	if (ep_sts_reg & EP_STS__TRBERR__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__TRBERR__MASK);
+
+		dev_dbg(&usb_ss->dev, "TRBERR(%02X)\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress);
+	}
+
+	if (ep_sts_reg & EP_STS__ISOERR__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__ISOERR__MASK);
+		dev_dbg(&usb_ss->dev, "ISOERR(%02X)\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress);
+	}
+
+	if (ep_sts_reg & EP_STS__OUTSMM__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__OUTSMM__MASK);
+		dev_dbg(&usb_ss->dev, "OUTSMM(%02X)\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress);
+	}
+
+	if (ep_sts_reg & EP_STS__NRDY__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__NRDY__MASK);
+		dev_dbg(&usb_ss->dev, "NRDY(%02X)\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress);
+	}
+
+	if ((ep_sts_reg & EP_STS__IOC__MASK) ||
+	    (ep_sts_reg & EP_STS__ISP__MASK)) {
+		u8 ep_dir;
+
+		cdns_flush_cache((uintptr_t)usb_ss_ep->trb_pool,
+				 sizeof(struct usb_ss_trb));
+
+		cdns_writel(&usb_ss->regs->ep_sts,
+			      EP_STS__IOC__MASK | EP_STS__ISP__MASK);
+
+		/* get just completed request */
+		request = next_request(&usb_ss_ep->request_list);
+		ep_dir = usb_ss_ep->endpoint.desc->bEndpointAddress;
+		usb_gadget_unmap_request(&usb_ss->gadget, request,
+					 ep_dir & ENDPOINT_DIR_MASK);
+
+		request->status = 0;
+		request->actual =
+			le32_to_cpu(((u32 *)usb_ss_ep->trb_pool)[1])
+			& ACTUAL_TRANSFERRED_BYTES_MASK;
+
+		dev_dbg(&usb_ss->dev, "IOC(%02X) %d\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress,
+			request->actual);
+
+		list_del(&request->list);
+
+		usb_ss_ep->hw_pending_flag = 0;
+		if (request->complete) {
+			spin_unlock(&usb_ss->lock);
+			usb_gadget_giveback_request(&usb_ss_ep->endpoint,
+						    request);
+			spin_lock(&usb_ss->lock);
+		}
+
+		/* handle deferred STALL */
+		if (usb_ss_ep->stalled_flag) {
+			cdns_ep_stall_flush(usb_ss_ep);
+			return 0;
+		}
+
+		/* exit if hardware transfer already started */
+		if (usb_ss_ep->hw_pending_flag)
+			return 0;
+
+		/* if any request queued run it! */
+		if (!list_empty(&usb_ss_ep->request_list))
+			cdns_ep_run_transfer(usb_ss_ep);
+	}
+
+	if (ep_sts_reg & EP_STS__DESCMIS__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__DESCMIS__MASK);
+		dev_dbg(&usb_ss->dev, "DESCMIS(%02X)\n",
+			usb_ss_ep->endpoint.desc->bEndpointAddress);
+	}
+
+	return 0;
+}
+
+/**
+ * cdns_check_ep0_interrupt_proceed - Processes interrupt related to endpoint 0
+ * @usb_ss: extended gadget object
+ * @dir: 1 for IN direction, 0 for OUT direction
+ */
+static void cdns_check_ep0_interrupt_proceed(struct usb_ss_dev *usb_ss, int dir)
+{
+	u32 ep_sts_reg;
+	int i;
+
+	select_ep(usb_ss, 0 | (dir ? USB_DIR_IN : USB_DIR_OUT));
+	ep_sts_reg = cdns_readl(&usb_ss->regs->ep_sts);
+
+	dev_dbg(&usb_ss->dev, "EP_STS: %08X\n", ep_sts_reg);
+
+	if ((ep_sts_reg & EP_STS__SETUP__MASK) && dir == 0) {
+		cdns_flush_cache((uintptr_t)usb_ss->setup, 8);
+
+		dev_dbg(&usb_ss->dev, "SETUP(%02X)\n", 0x00);
+
+		cdns_writel(&usb_ss->regs->ep_sts,
+			      EP_STS__SETUP__MASK |
+			      EP_STS__IOC__MASK | EP_STS__ISP__MASK);
+
+		dev_dbg(&usb_ss->dev, "SETUP: ");
+		for (i = 0; i < 8; i++)
+			dev_dbg(&usb_ss->dev, "%02X ", usb_ss->setup[i]);
+		dev_dbg(&usb_ss->dev, "\nSTATE: %d\n", usb_ss->gadget.state);
+		usb_ss->ep0_data_dir = usb_ss->setup[0] & USB_DIR_IN;
+		cdns_ep0_setup_phase(usb_ss);
+		ep_sts_reg &= ~(EP_STS__SETUP__MASK |
+			EP_STS__IOC__MASK |
+			EP_STS__ISP__MASK);
+	}
+
+	if (ep_sts_reg & EP_STS__TRBERR__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__TRBERR__MASK);
+		dev_dbg(&usb_ss->dev, "TRBERR(%02X)\n",
+			dir ? USB_DIR_IN : USB_DIR_OUT);
+	}
+
+	if (ep_sts_reg & EP_STS__DESCMIS__MASK) {
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__DESCMIS__MASK);
+
+		dev_dbg(&usb_ss->dev, "DESCMIS(%02X)\n",
+			dir ? USB_DIR_IN : USB_DIR_OUT);
+
+		if (dir == 0 && !usb_ss->setup_pending) {
+			usb_ss->ep0_data_dir = 0;
+			cdns_ep0_run_transfer(usb_ss,
+					      usb_ss->setup_dma, 8, 0);
+		}
+	}
+
+	if ((ep_sts_reg & EP_STS__IOC__MASK) ||
+	    (ep_sts_reg & EP_STS__ISP__MASK)) {
+		cdns_flush_cache((uintptr_t)usb_ss->trb_ep0, 20);
+
+		cdns_writel(&usb_ss->regs->ep_sts, EP_STS__IOC__MASK);
+		if (usb_ss->actual_ep0_request) {
+			usb_gadget_unmap_request(&usb_ss->gadget,
+						 usb_ss->actual_ep0_request,
+						 usb_ss->ep0_data_dir);
+
+			usb_ss->actual_ep0_request->actual =
+				le32_to_cpu((usb_ss->trb_ep0)[1])
+				& ACTUAL_TRANSFERRED_BYTES_MASK;
+
+			dev_dbg(&usb_ss->dev, "IOC(%02X) %d\n",
+				dir ? USB_DIR_IN : USB_DIR_OUT,
+				usb_ss->actual_ep0_request->actual);
+			list_del_init(&usb_ss->actual_ep0_request->list);
+		}
+
+		if (usb_ss->actual_ep0_request &&
+		    usb_ss->actual_ep0_request->complete) {
+			spin_unlock(&usb_ss->lock);
+			usb_ss->actual_ep0_request->complete(usb_ss->gadget.ep0,
+						usb_ss->actual_ep0_request);
+			spin_lock(&usb_ss->lock);
+		}
+		cdns_prepare_setup_packet(usb_ss);
+		cdns_writel(&usb_ss->regs->ep_cmd, EP_CMD__REQ_CMPL__MASK);
+	}
+}
+
+/**
+ * cdns_check_usb_interrupt_proceed - Processes interrupt related to device
+ * @usb_ss: extended gadget object
+ * @usb_ists: bitmap representation of device's reported interrupts
+ * (usb_ists register value)
+ */
+static void cdns_check_usb_interrupt_proceed(struct usb_ss_dev *usb_ss,
+					     u32 usb_ists)
+{
+	int interrupt_bit = ffs(usb_ists) - 1;
+	int speed;
+	u32 val;
+
+	dev_dbg(&usb_ss->dev, "USB interrupt detected\n");
+
+	switch (interrupt_bit) {
+	case USB_ISTS__CON2I__SHIFT:
+		/* FS/HS Connection detected */
+		dev_dbg(&usb_ss->dev,
+			"[Interrupt] FS/HS Connection detected\n");
+		val = cdns_readl(&usb_ss->regs->usb_sts);
+		speed = USB_STS__USBSPEED__READ(val);
+		if (speed == USB_SPEED_WIRELESS)
+			speed = USB_SPEED_SUPER;
+		dev_dbg(&usb_ss->dev, "Speed value: %s (%d), usbsts:0x%x\n",
+			usb_speed_string(speed), speed, val);
+		usb_ss->gadget.speed = speed;
+		usb_ss->is_connected = 1;
+		usb_gadget_set_state(&usb_ss->gadget, USB_STATE_POWERED);
+		cdns_ep0_config(usb_ss);
+		break;
+
+	case USB_ISTS__CONI__SHIFT:
+		/* SS Connection detected */
+		dev_dbg(&usb_ss->dev, "[Interrupt] SS Connection detected\n");
+		val = cdns_readl(&usb_ss->regs->usb_sts);
+		speed = USB_STS__USBSPEED__READ(val);
+		if (speed == USB_SPEED_WIRELESS)
+			speed = USB_SPEED_SUPER;
+		dev_dbg(&usb_ss->dev, "Speed value: %s (%d), usbsts:0x%x\n",
+			usb_speed_string(speed), speed, val);
+		usb_ss->gadget.speed = speed;
+		usb_ss->is_connected = 1;
+		usb_gadget_set_state(&usb_ss->gadget, USB_STATE_POWERED);
+		cdns_ep0_config(usb_ss);
+		break;
+
+	case USB_ISTS__DIS2I__SHIFT:
+	case USB_ISTS__DISI__SHIFT:
+		/* SS Disconnection detected */
+		val = cdns_readl(&usb_ss->regs->usb_sts);
+		dev_dbg(&usb_ss->dev,
+			"[Interrupt] Disconnection detected: usbsts:0x%x\n",
+			val);
+		if (usb_ss->gadget_driver &&
+		    usb_ss->gadget_driver->disconnect) {
+			spin_unlock(&usb_ss->lock);
+			usb_ss->gadget_driver->disconnect(&usb_ss->gadget);
+			spin_lock(&usb_ss->lock);
+		}
+		usb_ss->gadget.speed = USB_SPEED_UNKNOWN;
+		usb_gadget_set_state(&usb_ss->gadget, USB_STATE_NOTATTACHED);
+		usb_ss->is_connected = 0;
+		cdns_gadget_unconfig(usb_ss);
+		break;
+
+	case USB_ISTS__L2ENTI__SHIFT:
+		dev_dbg(&usb_ss->dev,
+			"[Interrupt] Device suspended\n");
+		break;
+
+	case USB_ISTS__L2EXTI__SHIFT:
+		dev_dbg(&usb_ss->dev, "[Interrupt] L2 exit detected\n");
+		/*
+		 * Exit from standby mode
+		 * on L2 exit (Suspend in HS/FS or SS)
+		 */
+		break;
+	case USB_ISTS__U3EXTI__SHIFT:
+		/*
+		 * Exit from standby mode
+		 * on U3 exit (Suspend in HS/FS or SS)
+		 */
+		dev_dbg(&usb_ss->dev, "[Interrupt] U3 exit detected\n");
+		break;
+
+		/* resets cases */
+	case USB_ISTS__UWRESI__SHIFT:
+	case USB_ISTS__UHRESI__SHIFT:
+	case USB_ISTS__U2RESI__SHIFT:
+		dev_dbg(&usb_ss->dev, "[Interrupt] Reset detected\n");
+		val = cdns_readl(&usb_ss->regs->usb_sts);
+		speed = USB_STS__USBSPEED__READ(val);
+		if (speed == USB_SPEED_WIRELESS)
+			speed = USB_SPEED_SUPER;
+		usb_gadget_set_state(&usb_ss->gadget, USB_STATE_DEFAULT);
+		usb_ss->gadget.speed = speed;
+		cdns_gadget_unconfig(usb_ss);
+		cdns_ep0_config(usb_ss);
+		break;
+	default:
+		break;
+	}
+
+	/* Clear interrupt bit */
+	cdns_writel(&usb_ss->regs->usb_ists, (1uL << interrupt_bit));
+}
+
+#ifdef CDNS_THREADED_IRQ_HANDLING
+static irqreturn_t cdns_irq_handler(int irq, void *_usb_ss)
+{
+	struct usb_ss_dev *usb_ss = _usb_ss;
+
+	usb_ss->usb_ien = cdns_readl(&usb_ss->regs->usb_ien);
+	usb_ss->ep_ien = cdns_readl(&usb_ss->regs->ep_ien);
+
+	if (!cdns_readl(&usb_ss->regs->usb_ists) &&
+	    !cdns_readl(&usb_ss->regs->ep_ists)) {
+		dev_dbg(&usb_ss->dev, "--BUBBLE INTERRUPT 0 !!!\n");
+		if (cdns_readl(&usb_ss->regs->usb_sts) &
+		    USB_STS__CFGSTS__MASK)
+			return IRQ_HANDLED;
+		return IRQ_NONE;
+	}
+
+	cdns_writel(&usb_ss->regs->usb_ien, 0);
+	cdns_writel(&usb_ss->regs->ep_ien, 0);
+
+	cdns_readl(&usb_ss->regs->dma_axi_ctrl);
+	return IRQ_WAKE_THREAD;
+}
+#endif
+
+/**
+ * cdns_irq_handler - irq line interrupt handler
+ * @cdns: cdns3 instance
+ *
+ * Returns IRQ_HANDLED when interrupt raised by USBSS_DEV,
+ * IRQ_NONE when interrupt raised by other device connected
+ * to the irq line
+ */
+static int cdns_irq_handler_thread(struct cdns3 *cdns)
+{
+	struct usb_ss_dev *usb_ss =
+		container_of(cdns->gadget_dev, struct usb_ss_dev, dev);
+	u32 reg;
+	int ret = IRQ_NONE;
+	unsigned long flags;
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+
+	/* check USB device interrupt */
+	reg = cdns_readl(&usb_ss->regs->usb_ists);
+	if (reg) {
+		dev_dbg(&usb_ss->dev, "usb_ists: %08X\n", reg);
+		cdns_check_usb_interrupt_proceed(usb_ss, reg);
+		ret = IRQ_HANDLED;
+	}
+
+	/* check endpoint interrupt */
+	reg = cdns_readl(&usb_ss->regs->ep_ists);
+	if (reg != 0) {
+		dev_dbg(&usb_ss->dev, "ep_ists: %08X\n", reg);
+	} else {
+		if (cdns_readl(&usb_ss->regs->usb_sts) &
+				USB_STS__CFGSTS__MASK)
+			ret = IRQ_HANDLED;
+		goto irqend;
+	}
+
+	/* handle default endpoint OUT */
+	if (reg & EP_ISTS__EOUT0__MASK) {
+		cdns_check_ep0_interrupt_proceed(usb_ss, 0);
+		ret = IRQ_HANDLED;
+	}
+
+	/* handle default endpoint IN */
+	if (reg & EP_ISTS__EIN0__MASK) {
+		cdns_check_ep0_interrupt_proceed(usb_ss, 1);
+		ret = IRQ_HANDLED;
+	}
+
+	/* check if interrupt from non default endpoint, if no exit */
+	reg &= ~(EP_ISTS__EOUT0__MASK | EP_ISTS__EIN0__MASK);
+	if (!reg)
+		goto irqend;
+
+	do {
+		unsigned int bit_pos = ffs(reg);
+		u32 bit_mask = 1 << (bit_pos - 1);
+		u8 ep_index = CAST_EP_REG_POS_TO_INDEX(bit_pos);
+
+		dev_dbg(&usb_ss->dev, "Interrupt on index: %d bitmask %08X\n",
+			ep_index, bit_mask);
+		cdns_check_ep_interrupt_proceed(usb_ss->eps[ep_index]);
+		reg &= ~bit_mask;
+		ret = IRQ_HANDLED;
+	} while (reg);
+
+irqend:
+
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+#ifdef CDNS_THREADED_IRQ_HANDLING
+	local_irq_save(flags);
+	cdns_writel(&usb_ss->regs->usb_ien, usb_ss->usb_ien);
+	cdns_writel(&usb_ss->regs->ep_ien, usb_ss->ep_ien);
+	local_irq_restore(flags);
+#endif
+	return ret;
+}
+
+/**
+ * usb_ss_gadget_ep0_enable
+ * Function shouldn't be called by gadget driver,
+ * endpoint 0 is allways active
+ */
+static int usb_ss_gadget_ep0_enable(struct usb_ep *ep,
+				    const struct usb_endpoint_descriptor *desc)
+{
+	return -EINVAL;
+}
+
+/**
+ * usb_ss_gadget_ep0_disable
+ * Function shouldn't be called by gadget driver,
+ * endpoint 0 is allways active
+ */
+static int usb_ss_gadget_ep0_disable(struct usb_ep *ep)
+{
+	return -EINVAL;
+}
+
+/**
+ * usb_ss_gadget_ep0_set_halt
+ * @ep: pointer to endpoint zero object
+ * @value: 1 for set stall, 0 for clear stall
+ *
+ * Returns 0
+ */
+static int usb_ss_gadget_ep0_set_halt(struct usb_ep *ep, int value)
+{
+	/* TODO */
+	return 0;
+}
+
+/**
+ * usb_ss_gadget_ep0_queue Transfer data on endpoint zero
+ * @ep: pointer to endpoint zero object
+ * @request: pointer to request object
+ * @gfp_flags: gfp flags
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep0_queue(struct usb_ep *ep,
+				   struct usb_request *request,
+				   gfp_t gfp_flags)
+{
+	int ret;
+	unsigned long flags;
+	int erdy_sent = 0;
+	/* get extended endpoint */
+	struct usb_ss_endpoint *usb_ss_ep =
+		to_usb_ss_ep(ep);
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+
+	dev_dbg(&usb_ss->dev, "QUEUE(%02X) %d\n",
+		usb_ss->ep0_data_dir ? USB_DIR_IN : USB_DIR_OUT,
+		request->length);
+
+	/* send STATUS stage */
+	if (request->length == 0 && request->zero == 0) {
+		spin_lock_irqsave(&usb_ss->lock, flags);
+		select_ep(usb_ss, 0x00);
+		if (!usb_ss->hw_configured_flag) {
+			cdns_writel(&usb_ss->regs->usb_conf,
+				      USB_CONF__CFGSET__MASK);
+			/* SET CONFIGURATION */
+			cdns_prepare_setup_packet(usb_ss);
+			cdns_writel(&usb_ss->regs->ep_cmd,
+				      EP_CMD__ERDY__MASK
+				      | EP_CMD__REQ_CMPL__MASK);
+			/* wait until configuration set */
+			while (!(cdns_readl(&usb_ss->regs->usb_sts)
+					& USB_STS__CFGSTS__MASK))
+				;
+			erdy_sent = 1;
+			usb_ss->hw_configured_flag = 1;
+
+			list_for_each_entry(ep, &usb_ss->gadget.ep_list,
+					    ep_list) {
+				if (to_usb_ss_ep(ep)->enabled)
+					cdns_ep_run_transfer(to_usb_ss_ep(ep));
+			}
+		}
+		if (!erdy_sent)
+			cdns_writel(&usb_ss->regs->ep_cmd,
+				      EP_CMD__ERDY__MASK
+				      | EP_CMD__REQ_CMPL__MASK);
+		if (request->complete)
+			request->complete(usb_ss->gadget.ep0, request);
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	ret = usb_gadget_map_request(&usb_ss->gadget, request,
+				     usb_ss->ep0_data_dir);
+	if (ret) {
+		dev_err(&usb_ss->dev, "failed to map request\n");
+		return -EINVAL;
+	}
+
+	usb_ss->actual_ep0_request = request;
+	cdns_ep0_run_transfer(usb_ss, request->dma, request->length, 1);
+	list_add_tail(&request->list, &usb_ss_ep->request_list);
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return 0;
+}
+
+/**
+ * cdns_ep_config Configure hardware endpoint
+ * @usb_ss_ep: extended endpoint object
+ */
+static void cdns_ep_config(struct usb_ss_endpoint *usb_ss_ep)
+{
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	u32 ep_cfg = 0;
+	u32 max_packet_size = 0;
+	u32 bEndpointAddress = usb_ss_ep->num | usb_ss_ep->dir;
+	u32 interrupt_mask = 0;
+	bool is_iso_ep = (usb_ss_ep->type == USB_ENDPOINT_XFER_ISOC);
+
+	dev_dbg(&usb_ss->dev,
+		"%s: %s addr=0x%x, speed %d, is_iso_ep %d\n", __func__,
+		usb_ss_ep->name, bEndpointAddress, usb_ss->gadget.speed,
+		is_iso_ep);
+
+	if (is_iso_ep) {
+		ep_cfg = EP_CFG__EPTYPE__WRITE(USB_ENDPOINT_XFER_ISOC);
+		interrupt_mask = INTERRUPT_MASK;
+	} else {
+		ep_cfg = EP_CFG__EPTYPE__WRITE(USB_ENDPOINT_XFER_BULK);
+	}
+
+	switch (usb_ss->gadget.speed) {
+	case USB_SPEED_UNKNOWN:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_0;
+		break;
+
+	case USB_SPEED_LOW:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_8;
+		break;
+
+	case USB_SPEED_FULL:
+		max_packet_size = (is_iso_ep ?
+			ENDPOINT_MAX_PACKET_SIZE_1023 :
+			ENDPOINT_MAX_PACKET_SIZE_64);
+		break;
+
+	case USB_SPEED_HIGH:
+		max_packet_size = (is_iso_ep ?
+			ENDPOINT_MAX_PACKET_SIZE_1024 :
+			ENDPOINT_MAX_PACKET_SIZE_512);
+		break;
+
+	case USB_SPEED_WIRELESS:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_512;
+		break;
+
+	case USB_SPEED_SUPER:
+		max_packet_size = ENDPOINT_MAX_PACKET_SIZE_1024;
+		break;
+	}
+
+	ep_cfg |= EP_CFG__MAXPKTSIZE__WRITE(max_packet_size);
+
+	if (is_iso_ep) {
+		ep_cfg |= EP_CFG__BUFFERING__WRITE(1);
+		ep_cfg |= EP_CFG__MAXBURST__WRITE(0);
+	} else {
+		ep_cfg |= EP_CFG__BUFFERING__WRITE(3);
+		ep_cfg |= EP_CFG__MAXBURST__WRITE(15);
+	}
+
+	select_ep(usb_ss, bEndpointAddress);
+	cdns_writel(&usb_ss->regs->ep_cfg, ep_cfg);
+	cdns_writel(&usb_ss->regs->ep_sts_en,
+		      EP_STS_EN__TRBERREN__MASK | interrupt_mask);
+
+	/* enable interrupt for selected endpoint */
+	ep_cfg = cdns_readl(&usb_ss->regs->ep_ien);
+	ep_cfg |= CAST_EP_ADDR_TO_BIT_POS(bEndpointAddress);
+	cdns_writel(&usb_ss->regs->ep_ien, ep_cfg);
+}
+
+/**
+ * usb_ss_gadget_ep_enable Enable endpoint
+ * @ep: endpoint object
+ * @desc: endpoint descriptor
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep_enable(struct usb_ep *ep,
+				   const struct usb_endpoint_descriptor *desc)
+{
+	struct usb_ss_endpoint *usb_ss_ep;
+	struct usb_ss_dev *usb_ss;
+	unsigned long flags;
+	unsigned long *dma_addr;
+	int ret;
+	u32 ep_cfg;
+
+	usb_ss_ep = to_usb_ss_ep(ep);
+	usb_ss = usb_ss_ep->usb_ss;
+
+	if (!ep || !desc || desc->bDescriptorType != USB_DT_ENDPOINT) {
+		dev_err(&usb_ss->dev, "usb-ss: invalid parameters\n");
+		return -EINVAL;
+	}
+
+	if (!desc->wMaxPacketSize) {
+		dev_err(&usb_ss->dev, "usb-ss: missing wMaxPacketSize\n");
+		return -EINVAL;
+	}
+
+	ret = usb_ss_allocate_trb_pool(usb_ss_ep);
+	if (ret)
+		return ret;
+
+	if (!usb_ss_ep->cpu_addr) {
+		dma_addr = (unsigned long *)&usb_ss_ep->dma_addr;
+		usb_ss_ep->cpu_addr = dma_alloc_coherent(4096, dma_addr);
+
+		if (!usb_ss_ep->cpu_addr)
+			return -ENOMEM;
+	}
+
+	dev_dbg(&usb_ss->dev, "Enabling endpoint: %s, addr=0x%x\n",
+		ep->name, desc->bEndpointAddress);
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	select_ep(usb_ss, desc->bEndpointAddress);
+	ep_cfg = cdns_readl(&usb_ss->regs->ep_cfg);
+	ep_cfg |= EP_CFG__ENABLE__MASK;
+	cdns_writel(&usb_ss->regs->ep_cfg, ep_cfg);
+
+	usb_ss_ep->enabled = 1;
+	ep->desc = desc;
+	usb_ss_ep->hw_pending_flag = 0;
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return 0;
+}
+
+static int usb_ss_gadget_match_ep(struct usb_gadget *gadget,
+				  struct usb_ep *ep,
+				  struct usb_endpoint_descriptor *desc)
+{
+	struct usb_ss_dev __maybe_unused *usb_ss = gadget_to_usb_ss(gadget);
+	struct usb_ss_endpoint *usb_ss_ep;
+	unsigned long flags;
+
+	usb_ss_ep = to_usb_ss_ep(ep);
+
+	dev_dbg(&usb_ss->dev, "match endpoint: %s\n", usb_ss_ep->name);
+
+	u8 num = simple_strtoul(&ep->name[2], NULL, 10);
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	usb_ss_ep->num  = num;
+	usb_ss_ep->used = true;
+	usb_ss_ep->endpoint.desc = desc;
+	usb_ss_ep->dir  = usb_endpoint_dir_in(desc) ? USB_DIR_IN : USB_DIR_OUT;
+	usb_ss_ep->type = usb_endpoint_type(desc);
+	usb_ss_ep->address = desc->bEndpointAddress;
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return 1;
+}
+
+static void usb_ss_free_trb_pool(struct usb_ss_endpoint *usb_ss_ep)
+{
+	if (usb_ss_ep->trb_pool) {
+		dma_free_coherent(usb_ss_ep->trb_pool);
+		usb_ss_ep->trb_pool = NULL;
+	}
+
+	if (usb_ss_ep->cpu_addr) {
+		dma_free_coherent(usb_ss_ep->cpu_addr);
+		usb_ss_ep->cpu_addr = NULL;
+	}
+}
+
+/**
+ * usb_ss_gadget_ep_disable Disable endpoint
+ * @ep: endpoint object
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep_disable(struct usb_ep *ep)
+{
+	struct usb_ss_endpoint *usb_ss_ep;
+	struct usb_ss_dev *usb_ss;
+	unsigned long flags;
+	int ret = 0;
+	struct usb_request *request;
+	u32 ep_cfg;
+
+	if (!ep) {
+		pr_debug("usb-ss: invalid parameters\n");
+		return -EINVAL;
+	}
+
+	usb_ss_ep = to_usb_ss_ep(ep);
+	usb_ss = usb_ss_ep->usb_ss;
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	if (!usb_ss->start_gadget) {
+		dev_dbg(&usb_ss->dev,
+			"Disabling endpoint at disconnection: %s\n", ep->name);
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+
+	dev_dbg(&usb_ss->dev,
+		"Disabling endpoint: %s\n", ep->name);
+
+	while (!list_empty(&usb_ss_ep->request_list)) {
+		request = next_request(&usb_ss_ep->request_list);
+		usb_gadget_unmap_request(&usb_ss->gadget, request,
+					 ep->desc->bEndpointAddress &
+					 USB_DIR_IN);
+		request->status = -ESHUTDOWN;
+		list_del(&request->list);
+		spin_unlock(&usb_ss->lock);
+		usb_gadget_giveback_request(ep, request);
+		spin_lock(&usb_ss->lock);
+	}
+
+	select_ep(usb_ss, ep->desc->bEndpointAddress);
+	ep_cfg = cdns_readl(&usb_ss->regs->ep_cfg);
+	ep_cfg &= ~EP_CFG__ENABLE__MASK;
+	cdns_writel(&usb_ss->regs->ep_cfg, ep_cfg);
+	ep->desc = NULL;
+	usb_ss_ep->enabled = 0;
+
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return ret;
+}
+
+/**
+ * usb_ss_gadget_ep_alloc_request Allocates request
+ * @ep: endpoint object associated with request
+ * @gfp_flags: gfp flags
+ *
+ * Returns allocated request address, NULL on allocation error
+ */
+static struct usb_request *usb_ss_gadget_ep_alloc_request(struct usb_ep *ep,
+							  gfp_t gfp_flags)
+{
+	struct usb_request *request;
+
+	request = kzalloc(sizeof(*request), gfp_flags);
+	if (!request)
+		return NULL;
+
+	return request;
+}
+
+/**
+ * usb_ss_gadget_ep_free_request Free memory occupied by request
+ * @ep: endpoint object associated with request
+ * @request: request to free memory
+ */
+static void usb_ss_gadget_ep_free_request(struct usb_ep *ep,
+					  struct usb_request *request)
+{
+	kfree(request);
+}
+
+/**
+ * usb_ss_gadget_ep_queue Transfer data on endpoint
+ * @ep: endpoint object
+ * @request: request object
+ * @gfp_flags: gfp flags
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep_queue(struct usb_ep *ep,
+				  struct usb_request *request, gfp_t gfp_flags)
+{
+	struct usb_ss_endpoint *usb_ss_ep =
+		to_usb_ss_ep(ep);
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	unsigned long flags;
+	int ret = 0;
+	int empty_list = 0;
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+
+	request->actual = 0;
+	request->status = -EINPROGRESS;
+
+	dev_dbg(&usb_ss->dev,
+		"Queuing endpoint: %s\n", usb_ss_ep->name);
+
+	dev_dbg(&usb_ss->dev, "QUEUE(%02X) %d\n",
+		ep->desc->bEndpointAddress, request->length);
+
+	ret = usb_gadget_map_request(&usb_ss->gadget, request,
+				     ep->desc->bEndpointAddress & USB_DIR_IN);
+
+	if (ret) {
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return ret;
+	}
+
+	empty_list = list_empty(&usb_ss_ep->request_list);
+	list_add_tail(&request->list, &usb_ss_ep->request_list);
+
+	if (!usb_ss->hw_configured_flag) {
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+
+	if (empty_list) {
+		if (!usb_ss_ep->stalled_flag)
+			cdns_ep_run_transfer(usb_ss_ep);
+	}
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return ret;
+}
+
+/**
+ * usb_ss_gadget_ep_dequeue Remove request from transfer queue
+ * @ep: endpoint object associated with request
+ * @request: request object
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep_dequeue(struct usb_ep *ep,
+				    struct usb_request *request)
+{
+	struct usb_ss_endpoint *usb_ss_ep =
+		to_usb_ss_ep(ep);
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	struct usb_request *req, *req_temp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	if (!usb_ss->start_gadget) {
+		dev_dbg(&usb_ss->dev,
+			"DEQUEUE at disconnection: %s\n", ep->name);
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+	dev_dbg(&usb_ss->dev, "DEQUEUE(%02X) %d\n",
+		usb_ss_ep->address, request->length);
+
+	list_for_each_entry_safe(req, req_temp,
+				 &usb_ss_ep->request_list, list) {
+		if (request == req) {
+			request->status = -ECONNRESET;
+			usb_gadget_unmap_request(&usb_ss->gadget, request,
+						 usb_ss_ep->address &
+						 USB_DIR_IN);
+			list_del_init(&request->list);
+			if (request->complete) {
+				spin_unlock(&usb_ss->lock);
+				usb_gadget_giveback_request
+					(&usb_ss_ep->endpoint, request);
+				spin_lock(&usb_ss->lock);
+			}
+			break;
+		}
+	}
+
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+	return 0;
+}
+
+/**
+ * usb_ss_gadget_ep_set_halt Sets/clears stall on selected endpoint
+ * @ep: endpoint object to set/clear stall on
+ * @value: 1 for set stall, 0 for clear stall
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_ep_set_halt(struct usb_ep *ep, int value)
+{
+	struct usb_ss_endpoint *usb_ss_ep =
+		to_usb_ss_ep(ep);
+	struct usb_ss_dev *usb_ss = usb_ss_ep->usb_ss;
+	unsigned long flags;
+
+	/* return error when endpoint disabled */
+	if (!usb_ss_ep->enabled)
+		return -EPERM;
+
+	/* if actual transfer is pending defer setting stall on this endpoint */
+	if (usb_ss_ep->hw_pending_flag && value) {
+		usb_ss_ep->stalled_flag = 1;
+		return 0;
+	}
+
+	dev_dbg(&usb_ss->dev, "HALT(%02X) %d\n", usb_ss_ep->address, value);
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+
+	select_ep(usb_ss, ep->desc->bEndpointAddress);
+	if (value) {
+		cdns_ep_stall_flush(usb_ss_ep);
+	} else {
+		/*
+		 * TODO:
+		 * epp->wedgeFlag = 0;
+		 */
+		usb_ss_ep->wedge_flag = 0;
+		cdns_writel(&usb_ss->regs->ep_cmd,
+			      EP_CMD__CSTALL__MASK | EP_CMD__EPRST__MASK);
+		/* wait for EPRST cleared */
+		while (cdns_readl(&usb_ss->regs->ep_cmd) &
+		       EP_CMD__EPRST__MASK)
+			;
+		usb_ss_ep->stalled_flag = 0;
+	}
+	usb_ss_ep->hw_pending_flag = 0;
+
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+
+	return 0;
+}
+
+/**
+ * usb_ss_gadget_ep_set_wedge Set wedge on selected endpoint
+ * @ep: endpoint object
+ *
+ * Returns 0
+ */
+static int usb_ss_gadget_ep_set_wedge(struct usb_ep *ep)
+{
+	struct usb_ss_endpoint *usb_ss_ep = to_usb_ss_ep(ep);
+	struct usb_ss_dev __maybe_unused *usb_ss = usb_ss_ep->usb_ss;
+
+	dev_dbg(&usb_ss->dev, "WEDGE(%02X)\n", usb_ss_ep->address);
+	usb_ss_gadget_ep_set_halt(ep, 1);
+	usb_ss_ep->wedge_flag = 1;
+	return 0;
+}
+
+static const struct usb_ep_ops usb_ss_gadget_ep0_ops = {
+	.enable = usb_ss_gadget_ep0_enable,
+	.disable = usb_ss_gadget_ep0_disable,
+	.alloc_request = usb_ss_gadget_ep_alloc_request,
+	.free_request = usb_ss_gadget_ep_free_request,
+	.queue = usb_ss_gadget_ep0_queue,
+	.dequeue = usb_ss_gadget_ep_dequeue,
+	.set_halt = usb_ss_gadget_ep0_set_halt,
+	.set_wedge = usb_ss_gadget_ep_set_wedge,
+};
+
+static const struct usb_ep_ops usb_ss_gadget_ep_ops = {
+	.enable = usb_ss_gadget_ep_enable,
+	.disable = usb_ss_gadget_ep_disable,
+	.alloc_request = usb_ss_gadget_ep_alloc_request,
+	.free_request = usb_ss_gadget_ep_free_request,
+	.queue = usb_ss_gadget_ep_queue,
+	.dequeue = usb_ss_gadget_ep_dequeue,
+	.set_halt = usb_ss_gadget_ep_set_halt,
+	.set_wedge = usb_ss_gadget_ep_set_wedge,
+};
+
+/**
+ * usb_ss_gadget_get_frame Returns number of actual ITP frame
+ * @gadget: gadget object
+ *
+ * Returns number of actual ITP frame
+ */
+static int usb_ss_gadget_get_frame(struct usb_gadget *gadget)
+{
+	struct usb_ss_dev *usb_ss = gadget_to_usb_ss(gadget);
+
+	dev_dbg(&usb_ss->dev, "%s\n", __func__);
+	return cdns_readl(&usb_ss->regs->usb_iptn);
+}
+
+static int usb_ss_gadget_wakeup(struct usb_gadget *gadget)
+{
+	struct usb_ss_dev __maybe_unused *usb_ss = gadget_to_usb_ss(gadget);
+
+	dev_dbg(&usb_ss->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int usb_ss_gadget_set_selfpowered(struct usb_gadget *gadget,
+					 int is_selfpowered)
+{
+	struct usb_ss_dev __maybe_unused *usb_ss = gadget_to_usb_ss(gadget);
+
+	dev_dbg(&usb_ss->dev, "%s: %d\n", __func__, is_selfpowered);
+	return 0;
+}
+
+static int usb_ss_gadget_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct usb_ss_dev *usb_ss = gadget_to_usb_ss(gadget);
+
+	if (!usb_ss->start_gadget)
+		return 0;
+
+	dev_dbg(&usb_ss->dev, "%s: %d\n", __func__, is_on);
+
+	if (is_on)
+		cdns_writel(&usb_ss->regs->usb_conf, USB_CONF__DEVEN__MASK);
+	else
+		cdns_writel(&usb_ss->regs->usb_conf, USB_CONF__DEVDS__MASK);
+
+	return 0;
+}
+
+/**
+ * usb_ss_gadget_udc_start Gadget start
+ * @gadget: gadget object
+ * @driver: driver which operates on this gadget
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_gadget_udc_start(struct usb_gadget *gadget,
+				   struct usb_gadget_driver *driver)
+{
+	struct usb_ss_dev *usb_ss = gadget_to_usb_ss(gadget);
+	unsigned long flags;
+
+	if (usb_ss->gadget_driver) {
+		dev_err(&usb_ss->dev, "%s is already bound\n",
+			usb_ss->gadget.name);
+		return -EBUSY;
+	}
+
+	dev_dbg(&usb_ss->dev, "%s begins\n", __func__);
+
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	usb_ss->gadget_driver = driver;
+	if (!usb_ss->start_gadget) {
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+
+	__cdns3_gadget_start(usb_ss);
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+	dev_dbg(&usb_ss->dev, "%s ends\n", __func__);
+
+	return 0;
+}
+
+/**
+ * usb_ss_gadget_udc_stop Stops gadget
+ * @gadget: gadget object
+ *
+ * Returns 0
+ */
+static int usb_ss_gadget_udc_stop(struct usb_gadget *gadget)
+{
+	struct usb_ss_dev *usb_ss = gadget_to_usb_ss(gadget);
+	struct usb_ep *ep;
+	struct usb_ss_endpoint *usb_ss_ep;
+	int i;
+	u32 bEndpointAddress;
+
+	usb_ss->gadget_driver = NULL;
+	if (!usb_ss->start_gadget)
+		return 0;
+
+	list_for_each_entry(ep, &usb_ss->gadget.ep_list, ep_list) {
+		usb_ss_ep = to_usb_ss_ep(ep);
+		bEndpointAddress = usb_ss_ep->num | usb_ss_ep->dir;
+		usb_ss_ep->used = false;
+		select_ep(usb_ss, bEndpointAddress);
+		cdns_writel(&usb_ss->regs->ep_cmd, EP_CMD__EPRST__MASK);
+		while (cdns_readl(&usb_ss->regs->ep_cmd)
+			& EP_CMD__EPRST__MASK)
+			;
+	}
+
+	/* disable interrupt for device */
+	cdns_writel(&usb_ss->regs->usb_ien, 0);
+	cdns_writel(&usb_ss->regs->usb_conf, USB_CONF__DEVDS__MASK);
+
+	for (i = 0; i < usb_ss->ep_nums ; i++)
+		usb_ss_free_trb_pool(usb_ss->eps[i]);
+
+	return 0;
+}
+
+static void cdns3_gadget_set_speed(struct usb_gadget *g,
+				   enum usb_device_speed speed)
+{
+	struct usb_ss_dev *usb_ss = gadget_to_usb_ss(g);
+
+	usb_ss->gadget.max_speed = speed;
+}
+
+static const struct usb_gadget_ops usb_ss_gadget_ops = {
+	.get_frame = usb_ss_gadget_get_frame,
+	.wakeup = usb_ss_gadget_wakeup,
+	.set_selfpowered = usb_ss_gadget_set_selfpowered,
+	.pullup = usb_ss_gadget_pullup,
+	.udc_start = usb_ss_gadget_udc_start,
+	.udc_stop = usb_ss_gadget_udc_stop,
+	.match_ep = usb_ss_gadget_match_ep,
+	.udc_set_speed = cdns3_gadget_set_speed,
+
+};
+
+/**
+ * usb_ss_init_ep Initializes software endpoints of gadget
+ * @usb_ss: extended gadget object
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_init_ep(struct usb_ss_dev *usb_ss)
+{
+	struct usb_ss_endpoint *usb_ss_ep;
+	u32 ep_enabled_reg, iso_ep_reg, bulk_ep_reg;
+	int i;
+	int ep_reg_pos, ep_dir, ep_number;
+	int found_endpoints = 0;
+
+	/* Read it from USB_CAP3 to USB_CAP5 */
+	ep_enabled_reg = 0x00ff00ff;
+	iso_ep_reg = 0x00fe00fe;
+	bulk_ep_reg = 0x00fe00fe;
+
+	dev_dbg(&usb_ss->dev, "Initializing non-zero endpoints\n");
+	dev_dbg(&usb_ss->dev,
+		"ep_enabled_reg: 0x%x, iso_ep_reg: 0x%x, bulk_ep_reg:0x%x\n",
+		ep_enabled_reg, iso_ep_reg, bulk_ep_reg);
+
+	for (i = 0; i < USB_SS_ENDPOINTS_MAX_COUNT; i++) {
+		ep_number = (i / 2) + 1;
+		ep_dir = i % 2;
+		ep_reg_pos = (16 * ep_dir) + ep_number;
+
+		if (!(ep_enabled_reg & (1uL << ep_reg_pos)))
+			continue;
+
+		/* create empty endpoint object */
+		usb_ss_ep = devm_kzalloc(&usb_ss->dev, sizeof(*usb_ss_ep),
+					 GFP_KERNEL);
+		if (!usb_ss_ep)
+			return -ENOMEM;
+
+		/* set parent of endpoint object */
+		usb_ss_ep->usb_ss = usb_ss;
+
+		/* set index of endpoint in endpoints container */
+		usb_ss->eps[found_endpoints++] = usb_ss_ep;
+
+		/* set name of endpoint */
+		snprintf(usb_ss_ep->name, sizeof(usb_ss_ep->name), "ep%d%s",
+			 ep_number, !!ep_dir ? "in" : "out");
+		usb_ss_ep->endpoint.name = usb_ss_ep->name;
+		dev_dbg(&usb_ss->dev, "Initializing endpoint: %s\n",
+			usb_ss_ep->name);
+
+		usb_ep_set_maxpacket_limit(&usb_ss_ep->endpoint,
+					   ENDPOINT_MAX_PACKET_LIMIT);
+		usb_ss_ep->endpoint.max_streams = ENDPOINT_MAX_STREAMS;
+		usb_ss_ep->endpoint.ops = &usb_ss_gadget_ep_ops;
+		if (ep_dir)
+			usb_ss_ep->caps.dir_in = 1;
+		else
+			usb_ss_ep->caps.dir_out = 1;
+
+		/* check endpoint type */
+		if (iso_ep_reg & (1uL << ep_reg_pos))
+			usb_ss_ep->caps.type_iso = 1;
+
+		if (bulk_ep_reg & (1uL << ep_reg_pos)) {
+			usb_ss_ep->caps.type_bulk = 1;
+			usb_ss_ep->caps.type_int = 1;
+			usb_ss_ep->endpoint.maxburst = 15;
+		}
+
+		list_add_tail(&usb_ss_ep->endpoint.ep_list,
+			      &usb_ss->gadget.ep_list);
+		INIT_LIST_HEAD(&usb_ss_ep->request_list);
+		INIT_LIST_HEAD(&usb_ss_ep->ep_match_pending_list);
+	}
+	usb_ss->ep_nums = found_endpoints;
+
+	return 0;
+}
+
+/**
+ * usb_ss_init_ep0 Initializes software endpoint 0 of gadget
+ * @usb_ss: extended gadget object
+ *
+ * Returns 0 on success, error code elsewhere
+ */
+static int usb_ss_init_ep0(struct usb_ss_dev *usb_ss)
+{
+	struct usb_ss_endpoint *ep0;
+
+	dev_dbg(&usb_ss->dev, "Initializing EP0\n");
+	ep0 = devm_kzalloc(&usb_ss->dev, sizeof(struct usb_ss_endpoint),
+			   GFP_KERNEL);
+
+	if (!ep0)
+		return -ENOMEM;
+
+	/* fill CDNS fields */
+	ep0->usb_ss = usb_ss;
+	sprintf(ep0->name, "ep0");
+
+	/* fill linux fields */
+	ep0->endpoint.ops = &usb_ss_gadget_ep0_ops;
+	ep0->endpoint.maxburst = 1;
+	usb_ep_set_maxpacket_limit(&ep0->endpoint, ENDPOINT0_MAX_PACKET_LIMIT);
+	ep0->address = 0;
+	ep0->enabled = 1;
+	ep0->caps.type_control = 1;
+	ep0->caps.dir_in = 1;
+	ep0->caps.dir_out = 1;
+	ep0->endpoint.name = ep0->name;
+	ep0->endpoint.desc = &cdns3_gadget_ep0_desc;
+	usb_ss->gadget.ep0 = &ep0->endpoint;
+	INIT_LIST_HEAD(&ep0->request_list);
+
+	return 0;
+}
+
+static int __cdns3_gadget_init(struct cdns3 *cdns)
+{
+	struct usb_ss_dev *usb_ss;
+	int ret;
+	struct udevice *dev;
+	struct cdns3_generic_peripheral *priv = container_of(cdns,
+				struct cdns3_generic_peripheral, cdns3);
+
+	usb_ss = &priv->usb_ss_dev;
+	dev = &usb_ss->dev;
+	dev->parent = cdns->dev;
+	dev_set_name(dev, "gadget-cdns3-dev");
+	cdns->gadget_dev = dev;
+	usb_ss->sysdev = cdns->dev;
+	ret = device_register(dev);
+	if (ret)
+		goto err1;
+
+	usb_ss->regs = cdns->dev_regs;
+
+	/* fill gadget fields */
+	usb_ss->gadget.ops = &usb_ss_gadget_ops;
+	usb_ss->gadget.max_speed = USB_SPEED_SUPER;
+	usb_ss->gadget.speed = USB_SPEED_UNKNOWN;
+	usb_ss->gadget.name = "cdns3-gadget";
+	usb_ss->is_connected = 0;
+	spin_lock_init(&usb_ss->lock);
+
+	usb_ss->in_standby_mode = 1;
+
+	/* initialize endpoint container */
+	INIT_LIST_HEAD(&usb_ss->gadget.ep_list);
+	INIT_LIST_HEAD(&usb_ss->ep_match_list);
+	ret = usb_ss_init_ep0(usb_ss);
+	if (ret) {
+		dev_err(dev, "Failed to create endpoint 0\n");
+		ret = -ENOMEM;
+		goto err2;
+	}
+
+	ret = usb_ss_init_ep(usb_ss);
+	if (ret) {
+		dev_err(dev, "Failed to create non zero endpoints\n");
+		ret = -ENOMEM;
+		goto err2;
+	}
+
+	/* allocate memory for default endpoint TRB */
+	usb_ss->trb_ep0 = (u32 *)dma_alloc_coherent(20,
+				(unsigned long *)&usb_ss->trb_ep0_dma);
+	if (!usb_ss->trb_ep0) {
+		dev_err(dev, "Failed to allocate memory for ep0 TRB\n");
+		ret = -ENOMEM;
+		goto err2;
+	}
+
+	/* allocate memory for setup packet buffer */
+	usb_ss->setup = (u8 *)dma_alloc_coherent(8,
+			      (unsigned long *)&usb_ss->setup_dma);
+	if (!usb_ss->setup) {
+		dev_err(dev, "Failed to allocate memory for SETUP buffer\n");
+		ret = -ENOMEM;
+		goto err3;
+	}
+
+	/* add USB gadget device */
+	ret = usb_add_gadget_udc((struct device *)(&usb_ss->dev),
+				 &usb_ss->gadget);
+	if (ret < 0) {
+		dev_err(dev, "Failed to register USB device controller\n");
+		goto err4;
+	}
+
+	return 0;
+
+err4:
+	dma_free_coherent(usb_ss->setup);
+err3:
+	dma_free_coherent(usb_ss->trb_ep0);
+err2:
+err1:
+	cdns->gadget_dev = NULL;
+
+	return ret;
+}
+
+/**
+ * cdns3_gadget_remove: parent must call this to remove UDC
+ *
+ * cdns: cdns3 instance
+ *
+ */
+void cdns3_gadget_remove(struct cdns3 *cdns)
+{
+	struct usb_ss_dev *usb_ss;
+
+	if (!cdns->roles[CDNS3_ROLE_GADGET])
+		return;
+
+	usb_ss = container_of(cdns->gadget_dev, struct usb_ss_dev, dev);
+	usb_del_gadget_udc(&usb_ss->gadget);
+	dma_free_coherent(usb_ss->setup);
+	dma_free_coherent(usb_ss->trb_ep0);
+	device_unregister(cdns->gadget_dev);
+	cdns->gadget_dev = NULL;
+}
+
+static void __cdns3_gadget_start(struct usb_ss_dev *usb_ss)
+{
+	u32 usb_conf_reg = 0;
+
+	/* configure endpoint 0 hardware */
+	cdns_ep0_config(usb_ss);
+
+	/* enable interrupts for endpoint 0 (in and out) */
+	cdns_writel(&usb_ss->regs->ep_ien,
+		      EP_IEN__EOUTEN0__MASK | EP_IEN__EINEN0__MASK);
+
+	/* enable interrupt for device */
+	cdns_writel(&usb_ss->regs->usb_ien,
+		      USB_IEN__U2RESIEN__MASK
+		      | USB_ISTS__DIS2I__MASK
+		      | USB_IEN__CON2IEN__MASK
+		      | USB_IEN__UHRESIEN__MASK
+		      | USB_IEN__UWRESIEN__MASK
+		      | USB_IEN__DISIEN__MASK
+		      | USB_IEN__CONIEN__MASK
+		      | USB_IEN__U3EXTIEN__MASK
+		      | USB_IEN__L2ENTIEN__MASK
+		      | USB_IEN__L2EXTIEN__MASK);
+
+	usb_conf_reg = USB_CONF__CLK2OFFDS__MASK |
+			USB_CONF__L1DS__MASK;
+	if (usb_ss->gadget.max_speed == USB_SPEED_HIGH)
+		usb_conf_reg |= USB_CONF__USB3DIS__MASK;
+	cdns_writel(&usb_ss->regs->usb_conf, usb_conf_reg);
+
+	cdns_writel(&usb_ss->regs->usb_conf,
+		      USB_CONF__U1DS__MASK
+		      | USB_CONF__U2DS__MASK
+			/*
+			 * TODO:
+			 * | USB_CONF__L1EN__MASK
+			 */
+			);
+
+	cdns_writel(&usb_ss->regs->usb_conf, USB_CONF__DEVEN__MASK);
+
+	cdns_writel(&usb_ss->regs->dbg_link1,
+		      DBG_LINK1__LFPS_MIN_GEN_U1_EXIT_SET__MASK |
+		      DBG_LINK1__LFPS_MIN_GEN_U1_EXIT__WRITE(0x3C));
+}
+
+static int cdns3_gadget_start(struct cdns3 *cdns)
+{
+	struct usb_ss_dev *usb_ss = container_of(cdns->gadget_dev,
+			struct usb_ss_dev, dev);
+	unsigned long flags;
+
+	dev_dbg(&usb_ss->dev, "%s begins\n", __func__);
+
+	pm_runtime_get_sync(cdns->dev);
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	usb_ss->start_gadget = 1;
+	if (!usb_ss->gadget_driver) {
+		spin_unlock_irqrestore(&usb_ss->lock, flags);
+		return 0;
+	}
+
+	__cdns3_gadget_start(usb_ss);
+	usb_ss->in_standby_mode = 0;
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+	dev_dbg(&usb_ss->dev, "%s ends\n", __func__);
+
+	return 0;
+}
+
+static void __cdns3_gadget_stop(struct cdns3 *cdns)
+{
+	struct usb_ss_dev *usb_ss;
+	unsigned long flags;
+
+	usb_ss = container_of(cdns->gadget_dev, struct usb_ss_dev, dev);
+	if (usb_ss->gadget_driver)
+		usb_ss->gadget_driver->disconnect(&usb_ss->gadget);
+	usb_gadget_disconnect(&usb_ss->gadget);
+	spin_lock_irqsave(&usb_ss->lock, flags);
+	/* disable interrupt for device */
+	cdns_writel(&usb_ss->regs->usb_ien, 0);
+	cdns_writel(&usb_ss->regs->usb_conf, USB_CONF__DEVDS__MASK);
+	usb_ss->start_gadget = 0;
+	spin_unlock_irqrestore(&usb_ss->lock, flags);
+}
+
+static void cdns3_gadget_stop(struct cdns3 *cdns)
+{
+	if (cdns->role == CDNS3_ROLE_GADGET)
+		__cdns3_gadget_stop(cdns);
+}
+
+/**
+ * cdns3_gadget_init - initialize device structure
+ *
+ * cdns: cdns3 instance
+ *
+ * This function initializes the gadget.
+ */
+int cdns3_gadget_init(struct cdns3 *cdns)
+{
+	struct cdns3_role_driver *rdrv;
+
+	rdrv = devm_kzalloc(cdns->dev, sizeof(*rdrv), GFP_KERNEL);
+	if (!rdrv)
+		return -ENOMEM;
+
+	rdrv->start	= cdns3_gadget_start;
+	rdrv->stop	= cdns3_gadget_stop;
+	rdrv->irq	= cdns_irq_handler_thread;
+	rdrv->name	= "gadget";
+	cdns->roles[CDNS3_ROLE_GADGET] = rdrv;
+	return __cdns3_gadget_init(cdns);
+}
diff --git a/drivers/usb/cdns3/gadget.h b/drivers/usb/cdns3/gadget.h
new file mode 100644
index 0000000000..c645340f01
--- /dev/null
+++ b/drivers/usb/cdns3/gadget.h
@@ -0,0 +1,225 @@ 
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright (C) 2016 Cadence Design Systems - http://www.cadence.com
+ * Copyright 2019 NXP
+ */
+#ifndef __DRIVERS_CDNS3_GADGET
+#define __DRIVERS_CDNS3_GADGET
+
+#include "dev-regs-map.h"
+
+#if IS_ENABLED(CONFIG_USB_CDNS_MISC)
+#include "cdns_misc.h"
+#endif
+
+#include <dm.h>
+#include <clk.h>
+#include <generic-phy.h>
+
+#define gadget_to_usb_ss(g)  \
+	(container_of(g, struct usb_ss_dev, gadget))
+
+#define to_usb_ss_ep(ep) \
+	(container_of(ep, struct usb_ss_endpoint, endpoint))
+
+#define ep_to_usb_ss_ep(ep) \
+	(container_of(ep, struct usb_ss_endpoint, endpoint))
+
+/*-------------------------------------------------------------------------*/
+/* TRB macros */
+
+/* Common TRB fields */
+#define TRB_SET_CYCLE_BIT		1uL
+#define TRB_SET_CHAIN_BIT		0x10
+
+/* offset 0 */
+#define TRB_DATA_BUFFER_POINTER_MASK	0xFFFFFFFF
+#define TRB_SET_DATA_BUFFER_POINTER(p)	(p & TRB_DATA_BUFFER_POINTER_MASK)
+
+/* offset 4 */
+#define TRB_TRANSFER_LENGTH_MASK	0x1FFFF
+#define TRB_SET_TRANSFER_LENGTH(l)	(l & TRB_TRANSFER_LENGTH_MASK)
+
+#define TRB_BURST_LENGTH_MASK		0xFF
+#define TRB_SET_BURST_LENGTH(l)		((l & TRB_BURST_LENGTH_MASK) << 24)
+
+/* offset 8 */
+#define TRB_SET_INT_ON_SHORT_PACKET	0x04
+#define TRB_SET_FIFO_MODE		0x08
+#define TRB_SET_INT_ON_COMPLETION	0x20
+
+#define TRB_TYPE_NORMAL			0x400
+
+#define TRB_STREAM_ID_MASK		0xFFFF
+#define TRB_SET_STREAM_ID(sid)		((sid & TRB_STREAM_ID_MASK) << 16)
+
+/*-------------------------------------------------------------------------*/
+/* Driver numeric constants */
+
+#define DEVICE_ADDRESS_MAX		127
+
+/* Endpoint init values */
+#define ENDPOINT_MAX_PACKET_LIMIT	1024
+
+#define ENDPOINT_MAX_STREAMS		15
+
+#define ENDPOINT0_MAX_PACKET_LIMIT	512
+
+/* All endpoints except EP0 */
+#define USB_SS_ENDPOINTS_MAX_COUNT	30
+
+#define USB_SS_TRBS_NUM			32
+
+/* Standby mode */
+#define STB_CLK_SWITCH_DONE_MASK	0x200
+#define STB_CLK_SWITCH_EN_MASK		0x100
+#define STB_CLK_SWITCH_EN_SHIFT		8
+
+#define ENDPOINT_MAX_PACKET_SIZE_0	0
+#define ENDPOINT_MAX_PACKET_SIZE_8	8
+#define ENDPOINT_MAX_PACKET_SIZE_64	64
+#define ENDPOINT_MAX_PACKET_SIZE_512	512
+#define ENDPOINT_MAX_PACKET_SIZE_1023	1023
+#define ENDPOINT_MAX_PACKET_SIZE_1024	1024
+
+#define SS_LINK_STATE_U3		3
+#define FSHS_LPM_STATE_L2		2
+
+#define ADDR_MODULO_8			8
+
+#define INTERRUPT_MASK			0xFFFFFFFF
+
+#define ACTUAL_TRANSFERRED_BYTES_MASK	0x1FFFF
+
+#define ENDPOINT_DIR_MASK		0x80
+
+/*-------------------------------------------------------------------------*/
+
+/**
+ * IS_REG_REQUIRING_ACTIVE_REF_CLOCK - Macro checks if desired
+ * register requires active clock, it involves such registers as:
+ * EP_CFG, EP_TR_ADDR, EP_CMD, EP_SEL, USB_CONF
+ * @usb_ss: extended gadget object
+ * @reg: register address
+ */
+#define IS_REG_REQUIRING_ACTIVE_REF_CLOCK(usb_ss, reg)	(!(reg) || \
+	((reg) >= &(usb_ss)->regs->ep_sel && (reg) <= &(usb_ss)->regs->ep_cmd))
+
+/**
+ * CAST_EP_REG_POS_TO_INDEX - Macro converts bit position of ep_ists register to
+ * index of endpoint object in usb_ss_dev.eps[] container
+ * @i: bit position of endpoint for which endpoint object is required
+ *
+ * Remember that endpoint container doesn't contain default endpoint
+ */
+#define CAST_EP_REG_POS_TO_INDEX(i) (((i) / 16) + ((((i) % 16) - 2) * 2))
+
+/**
+ * CAST_EP_ADDR_TO_INDEX - Macro converts endpoint address to
+ * index of endpoint object in usb_ss_dev.eps[] container
+ * @ep_addr: endpoint address for which endpoint object is required
+ *
+ * Remember that endpoint container doesn't contain default endpoint
+ */
+#define CAST_EP_ADDR_TO_INDEX(ep_addr) \
+	((((ep_addr) & 0x7F) - 1) + (((ep_addr) & 0x80) ? 1 : 0))
+
+/**
+ * CAST_EP_ADDR_TO_BIT_POS - Macro converts endpoint address to
+ * bit position in ep_ists register
+ * @ep_addr: endpoint address for which bit position is required
+ *
+ * Remember that endpoint container doesn't contain default endpoint
+ */
+#define CAST_EP_ADDR_TO_BIT_POS(ep_addr) \
+	(((uint32_t)1 << ((ep_addr) & 0x7F))  << (((ep_addr) & 0x80) ? 16 : 0))
+
+#define CAST_INDEX_TO_EP_ADDR(index) \
+	(((index) / 2 + 1) | (((index) % 2) ? 0x80 : 0x00))
+
+/*-------------------------------------------------------------------------*/
+/* Used structs */
+
+struct usb_ss_trb {
+	u32 offset0;
+	u32 offset4;
+	u32 offset8;
+};
+
+struct usb_ss_dev;
+
+struct usb_ep_caps {
+	unsigned type_control:1;
+	unsigned type_iso:1;
+	unsigned type_bulk:1;
+	unsigned type_int:1;
+	unsigned dir_in:1;
+	unsigned dir_out:1;
+};
+
+struct usb_ss_endpoint {
+	struct usb_ep endpoint;
+	struct list_head request_list;
+	struct list_head ep_match_pending_list;
+
+	struct usb_ss_trb *trb_pool;
+	dma_addr_t trb_pool_dma;
+
+	struct usb_ss_dev *usb_ss;
+	char name[20];
+	int hw_pending_flag;
+	int stalled_flag;
+	int wedge_flag;
+	void *cpu_addr;
+	dma_addr_t dma_addr;
+	u8					dir;
+	u8					num;
+	u8					type;
+	u8					address;
+	bool				used;
+	bool				enabled;
+	struct usb_ep_caps	caps;
+};
+
+struct usb_ss_dev {
+	struct udevice dev;
+	struct usbss_dev_register_block_type __iomem *regs;
+
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *gadget_driver;
+
+	dma_addr_t setup_dma;
+	dma_addr_t trb_ep0_dma;
+	u32 *trb_ep0;
+	u8 *setup;
+
+	struct usb_ss_endpoint *eps[USB_SS_ENDPOINTS_MAX_COUNT];
+	int ep_nums;
+	struct usb_request *actual_ep0_request;
+	int ep0_data_dir;
+	int hw_configured_flag;
+	int wake_up_flag;
+	u16 isoch_delay;
+	spinlock_t lock;	/* protection lock */
+
+	unsigned is_connected:1;
+	unsigned in_standby_mode:1;
+
+	u32 usb_ien;
+	u32 ep_ien;
+	int setup_pending;
+	struct udevice *sysdev;
+	bool start_gadget; /* The device mode is enabled */
+	struct list_head ep_match_list;
+};
+
+struct cdns3_generic_peripheral {
+	struct cdns3 cdns3;
+	struct usb_ss_dev usb_ss_dev;
+	struct clk_bulk clks;
+	struct phy phy;
+};
+
+#define OTG_STS_SELECTOR	0xF000		/* OTG status selector */
+
+#endif /* __DRIVERS_CDNS3_GADGET */
diff --git a/drivers/usb/cdns3/io.h b/drivers/usb/cdns3/io.h
new file mode 100644
index 0000000000..22b1b03950
--- /dev/null
+++ b/drivers/usb/cdns3/io.h
@@ -0,0 +1,30 @@ 
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2016 Cadence Design Systems - https://www.cadence.com/
+ * Copyright 2019 NXP
+ */
+
+#ifndef __DRIVERS_USB_CDNS_IO_H
+#define __DRIVERS_USB_CDNS_IO_H
+
+#include <linux/io.h>
+
+static inline u32 cdns_readl(u32 __iomem *reg)
+{
+	u32 value = 0;
+
+	value = readl(reg);
+	return value;
+}
+
+static inline void cdns_writel(u32 __iomem *reg, u32 value)
+{
+	writel(value, reg);
+}
+
+static inline void cdns_flush_cache(uintptr_t addr, int length)
+{
+	flush_dcache_range(addr, addr + ROUND(length, ARCH_DMA_MINALIGN));
+}
+
+#endif /* __DRIVERS_USB_CDNS_IO_H */
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 179b94cdd0..360f2b75ff 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -167,6 +167,10 @@  static int ep_matches(
 			size = 64;
 		put_unaligned(cpu_to_le16(size), &desc->wMaxPacketSize);
 	}
+
+	if (gadget->ops->match_ep)
+		return gadget->ops->match_ep(gadget, ep, desc);
+
 	return 1;
 }
 
diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index 2c8f235d51..ba9114cf37 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -149,6 +149,11 @@ 
 #define gadget_is_dwc3(g)        0
 #endif
 
+#ifdef CONFIG_USB_CDNS3_GADGET
+#define gadget_is_cdns3(g)        (!strcmp("cdns3-gadget", (g)->name))
+#else
+#define gadget_is_cdns3(g)        0
+#endif
 /**
  * usb_gadget_controller_number - support bcdDevice id convention
  * @gadget: the controller being driven
@@ -208,5 +213,7 @@  static inline int usb_gadget_controller_number(struct usb_gadget *gadget)
 		return 0x22;
 	else if (gadget_is_dwc3(gadget))
 		return 0x23;
+	else if (gadget_is_cdns3(gadget))
+		return 0x24;
 	return -ENOENT;
 }
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index 95dbf0c82e..414edad921 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -4,6 +4,7 @@ 
 
 ifndef CONFIG_$(SPL_)DM_USB_GADGET
 obj-$(CONFIG_USB_DWC3_GADGET)	+= udc-core.o
+obj-$(CONFIG_USB_CDNS3_GADGET)	+= udc-core.o
 endif
 
 obj-$(CONFIG_$(SPL_)DM_USB_GADGET)	+= udc-core.o
diff --git a/include/linux/usb/gadget.h b/include/linux/usb/gadget.h
index 497798a32a..a34f3478f3 100644
--- a/include/linux/usb/gadget.h
+++ b/include/linux/usb/gadget.h
@@ -447,6 +447,9 @@  struct usb_gadget_ops {
 	int	(*udc_start)(struct usb_gadget *,
 			     struct usb_gadget_driver *);
 	int	(*udc_stop)(struct usb_gadget *);
+	int   (*match_ep)(struct usb_gadget *gadget,
+			  struct usb_ep *ep,
+			  struct usb_endpoint_descriptor *desc);
 };
 
 /**
diff --git a/scripts/Makefile.spl b/scripts/Makefile.spl
index 7af6b120b6..d731c21aa4 100644
--- a/scripts/Makefile.spl
+++ b/scripts/Makefile.spl
@@ -88,6 +88,7 @@  endif
 
 libs-y += drivers/
 libs-$(CONFIG_SPL_USB_GADGET) += drivers/usb/dwc3/
+libs-$(CONFIG_SPL_USB_GADGET) += drivers/usb/cdns3/
 libs-y += dts/
 libs-y += fs/
 libs-$(CONFIG_SPL_POST_MEM_SUPPORT) += post/drivers/