usb: cdnsp: fix lack of ZLP for ep0

[platform/kernel/linux-rpi.git] / drivers / usb / cdns3 / cdnsp-ring.c

diff --git a/drivers/usb/cdns3/cdnsp-ring.c b/drivers/usb/cdns3/cdnsp-ring.c
index 2f29431..b23e543 100644 (file)
--- a/drivers/usb/cdns3/cdnsp-ring.c
+++ b/drivers/usb/cdns3/cdnsp-ring.c
@@ -2006,10 +2006,11 @@ int cdnsp_queue_bulk_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq)
 
 int cdnsp_queue_ctrl_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq)
 {
-       u32 field, length_field, remainder;
+       u32 field, length_field, zlp = 0;
        struct cdnsp_ep *pep = preq->pep;
        struct cdnsp_ring *ep_ring;
        int num_trbs;
+       u32 maxp;
        int ret;
 
        ep_ring = cdnsp_request_to_transfer_ring(pdev, preq);
@@ -2019,26 +2020,33 @@ int cdnsp_queue_ctrl_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq)
        /* 1 TRB for data, 1 for status */
        num_trbs = (pdev->three_stage_setup) ? 2 : 1;
 
+       maxp = usb_endpoint_maxp(pep->endpoint.desc);
+
+       if (preq->request.zero && preq->request.length &&
+           (preq->request.length % maxp == 0)) {
+               num_trbs++;
+               zlp = 1;
+       }
+
        ret = cdnsp_prepare_transfer(pdev, preq, num_trbs);
        if (ret)
                return ret;
 
        /* If there's data, queue data TRBs */
-       if (pdev->ep0_expect_in)
-               field = TRB_TYPE(TRB_DATA) | TRB_IOC;
-       else
-               field = TRB_ISP | TRB_TYPE(TRB_DATA) | TRB_IOC;
-
        if (preq->request.length > 0) {
-               remainder = cdnsp_td_remainder(pdev, 0, preq->request.length,
-                                              preq->request.length, preq, 1, 0);
+               field = TRB_TYPE(TRB_DATA);
 
-               length_field = TRB_LEN(preq->request.length) |
-                               TRB_TD_SIZE(remainder) | TRB_INTR_TARGET(0);
+               if (zlp)
+                       field |= TRB_CHAIN;
+               else
+                       field |= TRB_IOC | (pdev->ep0_expect_in ? 0 : TRB_ISP);
 
                if (pdev->ep0_expect_in)
                        field |= TRB_DIR_IN;
 
+               length_field = TRB_LEN(preq->request.length) |
+                              TRB_TD_SIZE(zlp) | TRB_INTR_TARGET(0);
+
                cdnsp_queue_trb(pdev, ep_ring, true,
                                lower_32_bits(preq->request.dma),
                                upper_32_bits(preq->request.dma), length_field,
@@ -2046,6 +2054,20 @@ int cdnsp_queue_ctrl_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq)
                                TRB_SETUPID(pdev->setup_id) |
                                pdev->setup_speed);
 
+               if (zlp) {
+                       field = TRB_TYPE(TRB_NORMAL) | TRB_IOC;
+
+                       if (!pdev->ep0_expect_in)
+                               field = TRB_ISP;
+
+                       cdnsp_queue_trb(pdev, ep_ring, true,
+                                       lower_32_bits(preq->request.dma),
+                                       upper_32_bits(preq->request.dma), 0,
+                                       field | ep_ring->cycle_state |
+                                       TRB_SETUPID(pdev->setup_id) |
+                                       pdev->setup_speed);
+               }
+
                pdev->ep0_stage = CDNSP_DATA_STAGE;
        }