7 files changed, 276 insertions, 214 deletions

diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 7345d2115af2..480bd4d5710a 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -10,6 +10,7 @@ ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
 # Glue/Bridge layers go here
 
 obj-$(CONFIG_USB_CHIPIDEA)      += ci_hdrc_msm.o
+obj-$(CONFIG_USB_CHIPIDEA)      += ci_hdrc_zevio.o
 
 # PCI doesn't provide stubs, need to check
 ifneq ($(CONFIG_PCI),)
diff --git a/drivers/usb/chipidea/bits.h b/drivers/usb/chipidea/bits.h
index a85713165688..83d06c1455b7 100644
--- a/drivers/usb/chipidea/bits.h
+++ b/drivers/usb/chipidea/bits.h
@@ -50,12 +50,14 @@
 #define PORTSC_PTC            (0x0FUL << 16)
 #define PORTSC_PHCD(d)        ((d) ? BIT(22) : BIT(23))
 /* PTS and PTW for non lpm version only */
+#define PORTSC_PFSC           BIT(24)
 #define PORTSC_PTS(d)                                           \
         (u32)((((d) & 0x3) << 30) | (((d) & 0x4) ? BIT(25) : 0))
 #define PORTSC_PTW            BIT(28)
 #define PORTSC_STS            BIT(29)
 
 /* DEVLC */
+#define DEVLC_PFSC            BIT(23)
 #define DEVLC_PSPD            (0x03UL << 25)
 #define DEVLC_PSPD_HS         (0x02UL << 25)
 #define DEVLC_PTW             BIT(27)
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index 88b80f7728e4..e206406ae1d9 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -196,8 +196,6 @@ struct ci_hdrc {
 
         struct ci_hdrc_platform_data    *platdata;
         int                             vbus_active;
-        /* FIXME: some day, we'll not use global phy */
-        bool                            global_phy;
         struct usb_phy                  *transceiver;
         struct usb_hcd                  *hcd;
         struct dentry                   *debugfs;
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.c b/drivers/usb/chipidea/ci_hdrc_imx.c
index c00f77257d36..2e58f8dfd311 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.c
+++ b/drivers/usb/chipidea/ci_hdrc_imx.c
@@ -96,7 +96,7 @@ static int ci_hdrc_imx_probe(struct platform_device *pdev)
 {
         struct ci_hdrc_imx_data *data;
         struct ci_hdrc_platform_data pdata = {
-                .name           = "ci_hdrc_imx",
+                .name           = dev_name(&pdev->dev),
                 .capoffset      = DEF_CAPOFFSET,
                 .flags          = CI_HDRC_REQUIRE_TRANSCEIVER |
                                   CI_HDRC_DISABLE_STREAMING,
diff --git a/drivers/usb/chipidea/ci_hdrc_zevio.c b/drivers/usb/chipidea/ci_hdrc_zevio.c
new file mode 100644
index 000000000000..3bf6489ef5ec
--- /dev/null
+++ b/drivers/usb/chipidea/ci_hdrc_zevio.c
@@ -0,0 +1,72 @@
+/*
+ *      Copyright (C) 2013 Daniel Tang <tangrs@tangrs.id.au>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ * Based off drivers/usb/chipidea/ci_hdrc_msm.c
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/chipidea.h>
+
+#include "ci.h"
+
+static struct ci_hdrc_platform_data ci_hdrc_zevio_platdata = {
+        .name                   = "ci_hdrc_zevio",
+        .flags                  = CI_HDRC_REGS_SHARED,
+        .capoffset              = DEF_CAPOFFSET,
+};
+
+static int ci_hdrc_zevio_probe(struct platform_device *pdev)
+{
+        struct platform_device *ci_pdev;
+
+        dev_dbg(&pdev->dev, "ci_hdrc_zevio_probe\n");
+
+        ci_pdev = ci_hdrc_add_device(&pdev->dev,
+                                pdev->resource, pdev->num_resources,
+                                &ci_hdrc_zevio_platdata);
+
+        if (IS_ERR(ci_pdev)) {
+                dev_err(&pdev->dev, "ci_hdrc_add_device failed!\n");
+                return PTR_ERR(ci_pdev);
+        }
+
+        platform_set_drvdata(pdev, ci_pdev);
+
+        return 0;
+}
+
+static int ci_hdrc_zevio_remove(struct platform_device *pdev)
+{
+        struct platform_device *ci_pdev = platform_get_drvdata(pdev);
+
+        ci_hdrc_remove_device(ci_pdev);
+
+        return 0;
+}
+
+static const struct of_device_id ci_hdrc_zevio_dt_ids[] = {
+        { .compatible = "lsi,zevio-usb", },
+        { /* sentinel */ }
+};
+
+static struct platform_driver ci_hdrc_zevio_driver = {
+        .probe = ci_hdrc_zevio_probe,
+        .remove = ci_hdrc_zevio_remove,
+        .driver = {
+                .name = "zevio_usb",
+                .owner = THIS_MODULE,
+                .of_match_table = ci_hdrc_zevio_dt_ids,
+        },
+};
+
+MODULE_DEVICE_TABLE(of, ci_hdrc_zevio_dt_ids);
+module_platform_driver(ci_hdrc_zevio_driver);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 33f22bc6ad7f..ca6831c5b763 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -64,6 +64,7 @@
 #include <linux/usb/otg.h>
 #include <linux/usb/chipidea.h>
 #include <linux/usb/of.h>
+#include <linux/of.h>
 #include <linux/phy.h>
 #include <linux/regulator/consumer.h>
 
@@ -298,6 +299,13 @@ int hw_device_reset(struct ci_hdrc *ci, u32 mode)
         if (ci->platdata->flags & CI_HDRC_DISABLE_STREAMING)
                 hw_write(ci, OP_USBMODE, USBMODE_CI_SDIS, USBMODE_CI_SDIS);
 
+        if (ci->platdata->flags & CI_HDRC_FORCE_FULLSPEED) {
+                if (ci->hw_bank.lpm)
+                        hw_write(ci, OP_DEVLC, DEVLC_PFSC, DEVLC_PFSC);
+                else
+                        hw_write(ci, OP_PORTSC, PORTSC_PFSC, PORTSC_PFSC);
+        }
+
         /* USBMODE should be configured step by step */
         hw_write(ci, OP_USBMODE, USBMODE_CM, USBMODE_CM_IDLE);
         hw_write(ci, OP_USBMODE, USBMODE_CM, mode);
@@ -412,6 +420,9 @@ static int ci_get_platdata(struct device *dev,
                 }
         }
 
+        if (of_usb_get_maximum_speed(dev->of_node) == USB_SPEED_FULL)
+                platdata->flags |= CI_HDRC_FORCE_FULLSPEED;
+
         return 0;
 }
 
@@ -496,33 +507,6 @@ static void ci_get_otg_capable(struct ci_hdrc *ci)
         }
 }
 
-static int ci_usb_phy_init(struct ci_hdrc *ci)
-{
-        if (ci->platdata->phy) {
-                ci->transceiver = ci->platdata->phy;
-                return usb_phy_init(ci->transceiver);
-        } else {
-                ci->global_phy = true;
-                ci->transceiver = usb_get_phy(USB_PHY_TYPE_USB2);
-                if (IS_ERR(ci->transceiver))
-                        ci->transceiver = NULL;
-
-                return 0;
-        }
-}
-
-static void ci_usb_phy_destroy(struct ci_hdrc *ci)
-{
-        if (!ci->transceiver)
-                return;
-
-        otg_set_peripheral(ci->transceiver->otg, NULL);
-        if (ci->global_phy)
-                usb_put_phy(ci->transceiver);
-        else
-                usb_phy_shutdown(ci->transceiver);
-}
-
 static int ci_hdrc_probe(struct platform_device *pdev)
 {
         struct device   *dev = &pdev->dev;
@@ -532,7 +516,7 @@ static int ci_hdrc_probe(struct platform_device *pdev)
         int             ret;
         enum usb_dr_mode dr_mode;
 
-        if (!dev->platform_data) {
+        if (!dev_get_platdata(dev)) {
                 dev_err(dev, "platform data missing\n");
                 return -ENODEV;
         }
@@ -549,7 +533,7 @@ static int ci_hdrc_probe(struct platform_device *pdev)
         }
 
         ci->dev = dev;
-        ci->platdata = dev->platform_data;
+        ci->platdata = dev_get_platdata(dev);
         ci->imx28_write_fix = !!(ci->platdata->flags &
                 CI_HDRC_IMX28_WRITE_FIX);
 
@@ -561,7 +545,26 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 
         hw_phymode_configure(ci);
 
-        ret = ci_usb_phy_init(ci);
+        if (ci->platdata->phy)
+                ci->transceiver = ci->platdata->phy;
+        else
+                ci->transceiver = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
+
+        if (IS_ERR(ci->transceiver)) {
+                ret = PTR_ERR(ci->transceiver);
+                /*
+                 * if -ENXIO is returned, it means PHY layer wasn't
+                 * enabled, so it makes no sense to return -EPROBE_DEFER
+                 * in that case, since no PHY driver will ever probe.
+                 */
+                if (ret == -ENXIO)
+                        return ret;
+
+                dev_err(dev, "no usb2 phy configured\n");
+                return -EPROBE_DEFER;
+        }
+
+        ret = usb_phy_init(ci->transceiver);
         if (ret) {
                 dev_err(dev, "unable to init phy: %d\n", ret);
                 return ret;
@@ -572,8 +575,8 @@ static int ci_hdrc_probe(struct platform_device *pdev)
         ci->irq = platform_get_irq(pdev, 0);
         if (ci->irq < 0) {
                 dev_err(dev, "missing IRQ\n");
-                ret = -ENODEV;
-                goto destroy_phy;
+                ret = ci->irq;
+                goto deinit_phy;
         }
 
         ci_get_otg_capable(ci);
@@ -590,23 +593,12 @@ static int ci_hdrc_probe(struct platform_device *pdev)
                 ret = ci_hdrc_gadget_init(ci);
                 if (ret)
                         dev_info(dev, "doesn't support gadget\n");
-                if (!ret && ci->transceiver) {
-                        ret = otg_set_peripheral(ci->transceiver->otg,
-                                                        &ci->gadget);
-                        /*
-                         * If we implement all USB functions using chipidea drivers,
-                         * it doesn't need to call above API, meanwhile, if we only
-                         * use gadget function, calling above API is useless.
-                         */
-                        if (ret && ret != -ENOTSUPP)
-                                goto destroy_phy;
-                }
         }
 
         if (!ci->roles[CI_ROLE_HOST] && !ci->roles[CI_ROLE_GADGET]) {
                 dev_err(dev, "no supported roles\n");
                 ret = -ENODEV;
-                goto destroy_phy;
+                goto deinit_phy;
         }
 
         if (ci->is_otg) {
@@ -663,8 +655,8 @@ static int ci_hdrc_probe(struct platform_device *pdev)
         free_irq(ci->irq, ci);
 stop:
         ci_role_destroy(ci);
-destroy_phy:
-        ci_usb_phy_destroy(ci);
+deinit_phy:
+        usb_phy_shutdown(ci->transceiver);
 
         return ret;
 }
@@ -677,7 +669,8 @@ static int ci_hdrc_remove(struct platform_device *pdev)
         free_irq(ci->irq, ci);
         ci_role_destroy(ci);
         ci_hdrc_enter_lpm(ci, true);
-        ci_usb_phy_destroy(ci);
+        usb_phy_shutdown(ci->transceiver);
+        kfree(ci->hw_bank.regmap);
 
         return 0;
 }
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 4ab2cb62dfce..7739c64ef259 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -178,19 +178,6 @@ static int hw_ep_get_halt(struct ci_hdrc *ci, int num, int dir)
 }
 
 /**
- * hw_test_and_clear_setup_status: test & clear setup status (execute without
- *                                 interruption)
- * @n: endpoint number
- *
- * This function returns setup status
- */
-static int hw_test_and_clear_setup_status(struct ci_hdrc *ci, int n)
-{
-        n = ep_to_bit(ci, n);
-        return hw_test_and_clear(ci, OP_ENDPTSETUPSTAT, BIT(n));
-}
-
-/**
  * hw_ep_prime: primes endpoint (execute without interruption)
  * @num:     endpoint number
  * @dir:     endpoint direction
@@ -962,6 +949,156 @@ __acquires(hwep->lock)
 }
 
 /**
+ * isr_setup_packet_handler: setup packet handler
+ * @ci: UDC descriptor
+ *
+ * This function handles setup packet 
+ */
+static void isr_setup_packet_handler(struct ci_hdrc *ci)
+__releases(ci->lock)
+__acquires(ci->lock)
+{
+        struct ci_hw_ep *hwep = &ci->ci_hw_ep[0];
+        struct usb_ctrlrequest req;
+        int type, num, dir, err = -EINVAL;
+        u8 tmode = 0;
+
+        /*
+         * Flush data and handshake transactions of previous
+         * setup packet.
+         */
+        _ep_nuke(ci->ep0out);
+        _ep_nuke(ci->ep0in);
+
+        /* read_setup_packet */
+        do {
+                hw_test_and_set_setup_guard(ci);
+                memcpy(&req, &hwep->qh.ptr->setup, sizeof(req));
+        } while (!hw_test_and_clear_setup_guard(ci));
+
+        type = req.bRequestType;
+
+        ci->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
+
+        switch (req.bRequest) {
+        case USB_REQ_CLEAR_FEATURE:
+                if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+                                le16_to_cpu(req.wValue) ==
+                                USB_ENDPOINT_HALT) {
+                        if (req.wLength != 0)
+                                break;
+                        num  = le16_to_cpu(req.wIndex);
+                        dir = num & USB_ENDPOINT_DIR_MASK;
+                        num &= USB_ENDPOINT_NUMBER_MASK;
+                        if (dir) /* TX */
+                                num += ci->hw_ep_max / 2;
+                        if (!ci->ci_hw_ep[num].wedge) {
+                                spin_unlock(&ci->lock);
+                                err = usb_ep_clear_halt(
+                                        &ci->ci_hw_ep[num].ep);
+                                spin_lock(&ci->lock);
+                                if (err)
+                                        break;
+                        }
+                        err = isr_setup_status_phase(ci);
+                } else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE) &&
+                                le16_to_cpu(req.wValue) ==
+                                USB_DEVICE_REMOTE_WAKEUP) {
+                        if (req.wLength != 0)
+                                break;
+                        ci->remote_wakeup = 0;
+                        err = isr_setup_status_phase(ci);
+                } else {
+                        goto delegate;
+                }
+                break;
+        case USB_REQ_GET_STATUS:
+                if (type != (USB_DIR_IN|USB_RECIP_DEVICE)   &&
+                    type != (USB_DIR_IN|USB_RECIP_ENDPOINT) &&
+                    type != (USB_DIR_IN|USB_RECIP_INTERFACE))
+                        goto delegate;
+                if (le16_to_cpu(req.wLength) != 2 ||
+                    le16_to_cpu(req.wValue)  != 0)
+                        break;
+                err = isr_get_status_response(ci, &req);
+                break;
+        case USB_REQ_SET_ADDRESS:
+                if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
+                        goto delegate;
+                if (le16_to_cpu(req.wLength) != 0 ||
+                    le16_to_cpu(req.wIndex)  != 0)
+                        break;
+                ci->address = (u8)le16_to_cpu(req.wValue);
+                ci->setaddr = true;
+                err = isr_setup_status_phase(ci);
+                break;
+        case USB_REQ_SET_FEATURE:
+                if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
+                                le16_to_cpu(req.wValue) ==
+                                USB_ENDPOINT_HALT) {
+                        if (req.wLength != 0)
+                                break;
+                        num  = le16_to_cpu(req.wIndex);
+                        dir = num & USB_ENDPOINT_DIR_MASK;
+                        num &= USB_ENDPOINT_NUMBER_MASK;
+                        if (dir) /* TX */
+                                num += ci->hw_ep_max / 2;
+
+                        spin_unlock(&ci->lock);
+                        err = usb_ep_set_halt(&ci->ci_hw_ep[num].ep);
+                        spin_lock(&ci->lock);
+                        if (!err)
+                                isr_setup_status_phase(ci);
+                } else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE)) {
+                        if (req.wLength != 0)
+                                break;
+                        switch (le16_to_cpu(req.wValue)) {
+                        case USB_DEVICE_REMOTE_WAKEUP:
+                                ci->remote_wakeup = 1;
+                                err = isr_setup_status_phase(ci);
+                                break;
+                        case USB_DEVICE_TEST_MODE:
+                                tmode = le16_to_cpu(req.wIndex) >> 8;
+                                switch (tmode) {
+                                case TEST_J:
+                                case TEST_K:
+                                case TEST_SE0_NAK:
+                                case TEST_PACKET:
+                                case TEST_FORCE_EN:
+                                        ci->test_mode = tmode;
+                                        err = isr_setup_status_phase(
+                                                        ci);
+                                        break;
+                                default:
+                                        break;
+                                }
+                        default:
+                                goto delegate;
+                        }
+                } else {
+                        goto delegate;
+                }
+                break;
+        default:
+delegate:
+                if (req.wLength == 0)   /* no data phase */
+                        ci->ep0_dir = TX;
+
+                spin_unlock(&ci->lock);
+                err = ci->driver->setup(&ci->gadget, &req);
+                spin_lock(&ci->lock);
+                break;
+        }
+
+        if (err < 0) {
+                spin_unlock(&ci->lock);
+                if (usb_ep_set_halt(&hwep->ep))
+                        dev_err(ci->dev, "error: ep_set_halt\n");
+                spin_lock(&ci->lock);
+        }
+}
+
+/**
  * isr_tr_complete_handler: transaction complete interrupt handler
  * @ci: UDC descriptor
  *
@@ -972,12 +1109,10 @@ __releases(ci->lock)
 __acquires(ci->lock)
 {
         unsigned i;
-        u8 tmode = 0;
+        int err;
 
         for (i = 0; i < ci->hw_ep_max; i++) {
                 struct ci_hw_ep *hwep  = &ci->ci_hw_ep[i];
-                int type, num, dir, err = -EINVAL;
-                struct usb_ctrlrequest req;
 
                 if (hwep->ep.desc == NULL)
                         continue;   /* not configured */
@@ -997,148 +1132,10 @@ __acquires(ci->lock)
                         }
                 }
 
-                if (hwep->type != USB_ENDPOINT_XFER_CONTROL ||
-                    !hw_test_and_clear_setup_status(ci, i))
-                        continue;
-
-                if (i != 0) {
-                        dev_warn(ci->dev, "ctrl traffic at endpoint %d\n", i);
-                        continue;
-                }
-
-                /*
-                 * Flush data and handshake transactions of previous
-                 * setup packet.
-                 */
-                _ep_nuke(ci->ep0out);
-                _ep_nuke(ci->ep0in);
-
-                /* read_setup_packet */
-                do {
-                        hw_test_and_set_setup_guard(ci);
-                        memcpy(&req, &hwep->qh.ptr->setup, sizeof(req));
-                } while (!hw_test_and_clear_setup_guard(ci));
-
-                type = req.bRequestType;
-
-                ci->ep0_dir = (type & USB_DIR_IN) ? TX : RX;
-
-                switch (req.bRequest) {
-                case USB_REQ_CLEAR_FEATURE:
-                        if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
-                                        le16_to_cpu(req.wValue) ==
-                                        USB_ENDPOINT_HALT) {
-                                if (req.wLength != 0)
-                                        break;
-                                num  = le16_to_cpu(req.wIndex);
-                                dir = num & USB_ENDPOINT_DIR_MASK;
-                                num &= USB_ENDPOINT_NUMBER_MASK;
-                                if (dir) /* TX */
-                                        num += ci->hw_ep_max/2;
-                                if (!ci->ci_hw_ep[num].wedge) {
-                                        spin_unlock(&ci->lock);
-                                        err = usb_ep_clear_halt(
-                                                &ci->ci_hw_ep[num].ep);
-                                        spin_lock(&ci->lock);
-                                        if (err)
-                                                break;
-                                }
-                                err = isr_setup_status_phase(ci);
-                        } else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE) &&
-                                        le16_to_cpu(req.wValue) ==
-                                        USB_DEVICE_REMOTE_WAKEUP) {
-                                if (req.wLength != 0)
-                                        break;
-                                ci->remote_wakeup = 0;
-                                err = isr_setup_status_phase(ci);
-                        } else {
-                                goto delegate;
-                        }
-                        break;
-                case USB_REQ_GET_STATUS:
-                        if (type != (USB_DIR_IN|USB_RECIP_DEVICE)   &&
-                            type != (USB_DIR_IN|USB_RECIP_ENDPOINT) &&
-                            type != (USB_DIR_IN|USB_RECIP_INTERFACE))
-                                goto delegate;
-                        if (le16_to_cpu(req.wLength) != 2 ||
-                            le16_to_cpu(req.wValue)  != 0)
-                                break;
-                        err = isr_get_status_response(ci, &req);
-                        break;
-                case USB_REQ_SET_ADDRESS:
-                        if (type != (USB_DIR_OUT|USB_RECIP_DEVICE))
-                                goto delegate;
-                        if (le16_to_cpu(req.wLength) != 0 ||
-                            le16_to_cpu(req.wIndex)  != 0)
-                                break;
-                        ci->address = (u8)le16_to_cpu(req.wValue);
-                        ci->setaddr = true;
-                        err = isr_setup_status_phase(ci);
-                        break;
-                case USB_REQ_SET_FEATURE:
-                        if (type == (USB_DIR_OUT|USB_RECIP_ENDPOINT) &&
-                                        le16_to_cpu(req.wValue) ==
-                                        USB_ENDPOINT_HALT) {
-                                if (req.wLength != 0)
-                                        break;
-                                num  = le16_to_cpu(req.wIndex);
-                                dir = num & USB_ENDPOINT_DIR_MASK;
-                                num &= USB_ENDPOINT_NUMBER_MASK;
-                                if (dir) /* TX */
-                                        num += ci->hw_ep_max/2;
-
-                                spin_unlock(&ci->lock);
-                                err = usb_ep_set_halt(&ci->ci_hw_ep[num].ep);
-                                spin_lock(&ci->lock);
-                                if (!err)
-                                        isr_setup_status_phase(ci);
-                        } else if (type == (USB_DIR_OUT|USB_RECIP_DEVICE)) {
-                                if (req.wLength != 0)
-                                        break;
-                                switch (le16_to_cpu(req.wValue)) {
-                                case USB_DEVICE_REMOTE_WAKEUP:
-                                        ci->remote_wakeup = 1;
-                                        err = isr_setup_status_phase(ci);
-                                        break;
-                                case USB_DEVICE_TEST_MODE:
-                                        tmode = le16_to_cpu(req.wIndex) >> 8;
-                                        switch (tmode) {
-                                        case TEST_J:
-                                        case TEST_K:
-                                        case TEST_SE0_NAK:
-                                        case TEST_PACKET:
-                                        case TEST_FORCE_EN:
-                                                ci->test_mode = tmode;
-                                                err = isr_setup_status_phase(
-                                                                ci);
-                                                break;
-                                        default:
-                                                break;
-                                        }
-                                default:
-                                        goto delegate;
-                                }
-                        } else {
-                                goto delegate;
-                        }
-                        break;
-                default:
-delegate:
-                        if (req.wLength == 0)   /* no data phase */
-                                ci->ep0_dir = TX;
-
-                        spin_unlock(&ci->lock);
-                        err = ci->driver->setup(&ci->gadget, &req);
-                        spin_lock(&ci->lock);
-                        break;
-                }
-
-                if (err < 0) {
-                        spin_unlock(&ci->lock);
-                        if (usb_ep_set_halt(&hwep->ep))
-                                dev_err(ci->dev, "error: ep_set_halt\n");
-                        spin_lock(&ci->lock);
-                }
+                /* Only handle setup packet below */
+                if (i == 0 &&
+                        hw_test_and_clear(ci, OP_ENDPTSETUPSTAT, BIT(0)))
+                        isr_setup_packet_handler(ci);
         }
 }
 
@@ -1193,6 +1190,11 @@ static int ep_enable(struct usb_ep *ep,
 
         hwep->qh.ptr->td.next |= cpu_to_le32(TD_TERMINATE);   /* needed? */
 
+        if (hwep->num != 0 && hwep->type == USB_ENDPOINT_XFER_CONTROL) {
+                dev_err(hwep->ci->dev, "Set control xfer at non-ep0\n");
+                retval = -EINVAL;
+        }
+
         /*
          * Enable endpoints in the HW other than ep0 as ep0
          * is always enabled
@@ -1837,12 +1839,6 @@ void ci_hdrc_gadget_destroy(struct ci_hdrc *ci)
 
         dma_pool_destroy(ci->td_pool);
         dma_pool_destroy(ci->qh_pool);
-
-        if (ci->transceiver) {
-                otg_set_peripheral(ci->transceiver->otg, NULL);
-                if (ci->global_phy)
-                        usb_put_phy(ci->transceiver);
-        }
 }
 
 static int udc_id_switch_for_device(struct ci_hdrc *ci)