3 files changed, 21 insertions, 2 deletions

diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 475c9c114689..ade1b91b5ae7 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -43,7 +43,7 @@
  *
  * TODO List
  * - OTG
- * - Isochronous & Interrupt Traffic
+ * - Interrupt Traffic
  * - Handle requests which spawns into several TDs
  * - GET_STATUS(device) - always reports 0
  * - Gadget API (majority of optional features)
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index b501346484ae..8aed28855c04 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -466,6 +466,14 @@ static int _hardware_enqueue(struct ci13xxx_ep *mEp, struct ci13xxx_req *mReq)
         mEp->qh.ptr->td.token &=
                 cpu_to_le32(~(TD_STATUS_HALTED|TD_STATUS_ACTIVE));
 
+        if (mEp->type == USB_ENDPOINT_XFER_ISOC) {
+                u32 mul = mReq->req.length / mEp->ep.maxpacket;
+
+                if (mReq->req.length % mEp->ep.maxpacket)
+                        mul++;
+                mEp->qh.ptr->cap |= mul << __ffs(QH_MULT);
+        }
+
         wmb();   /* synchronize before ep prime */
 
         ret = hw_ep_prime(ci, mEp->num, mEp->dir,
@@ -678,6 +686,12 @@ static int _ep_queue(struct usb_ep *ep, struct usb_request *req,
                 }
         }
 
+        if (usb_endpoint_xfer_isoc(mEp->ep.desc) &&
+            mReq->req.length > (1 + mEp->ep.mult) * mEp->ep.maxpacket) {
+                dev_err(mEp->ci->dev, "request length too big for isochronous\n");
+                return -EMSGSIZE;
+        }
+
         /* first nuke then test link, e.g. previous status has not sent */
         if (!list_empty(&mReq->queue)) {
                 dev_err(mEp->ci->dev, "request already in queue\n");
@@ -1060,7 +1074,8 @@ static int ep_enable(struct usb_ep *ep,
         mEp->num  = usb_endpoint_num(desc);
         mEp->type = usb_endpoint_type(desc);
 
-        mEp->ep.maxpacket = usb_endpoint_maxp(desc);
+        mEp->ep.maxpacket = usb_endpoint_maxp(desc) & 0x07ff;
+        mEp->ep.mult = QH_ISO_MULT(usb_endpoint_maxp(desc));
 
         if (mEp->type == USB_ENDPOINT_XFER_CONTROL)
                 cap |= QH_IOS;
@@ -1246,6 +1261,9 @@ static int ep_set_halt(struct usb_ep *ep, int value)
         if (ep == NULL || mEp->ep.desc == NULL)
                 return -EINVAL;
 
+        if (usb_endpoint_xfer_isoc(mEp->ep.desc))
+                return -EOPNOTSUPP;
+
         spin_lock_irqsave(mEp->lock, flags);
 
 #ifndef STALL_IN
diff --git a/drivers/usb/chipidea/udc.h b/drivers/usb/chipidea/udc.h
index d12e8b59b110..a75724a19e1a 100644
--- a/drivers/usb/chipidea/udc.h
+++ b/drivers/usb/chipidea/udc.h
@@ -50,6 +50,7 @@ struct ci13xxx_qh {
 #define QH_MAX_PKT            (0x07FFUL << 16)
 #define QH_ZLT                BIT(29)
 #define QH_MULT               (0x0003UL << 30)
+#define QH_ISO_MULT(x)          ((x >> 11) & 0x03)
         /* 1 */
         u32 curr;
         /* 2 - 8 */