usb: cdnsp: : add scatter gather support for ISOC endpoint

Patch implements scatter gather support for isochronous endpoint.
This fix is forced by 'commit e81e7f9a0eb9
("usb: gadget: uvc: add scatter gather support")'.
After this fix CDNSP driver stop working with UVC class.

Signed-off-by: Pawel Laszczak <pawell@cadence.com>
Reviewed-by: Peter Chen <peter.chen@kernel.org>
Link: https://lore.kernel.org/r/20221222090934.145140-1-pawell@cadence.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/usb/cdns3/cdnsp-gadget.c b/drivers/usb/cdns3/cdnsp-gadget.c
index f9aa50ff14d42bfcdeed861f2e71d3581b5cf0e4..fff9ec9c391fae2deb8c43cde5574455ee08540a 100644 (file)
--- a/drivers/usb/cdns3/cdnsp-gadget.c
+++ b/drivers/usb/cdns3/cdnsp-gadget.c
@@ -378,7 +378,7 @@ int cdnsp_ep_enqueue(struct cdnsp_ep *pep, struct cdnsp_request *preq)
                ret = cdnsp_queue_bulk_tx(pdev, preq);
                break;
        case USB_ENDPOINT_XFER_ISOC:
-               ret = cdnsp_queue_isoc_tx_prepare(pdev, preq);
+               ret = cdnsp_queue_isoc_tx(pdev, preq);
        }
 
        if (ret)

diff --git a/drivers/usb/cdns3/cdnsp-gadget.h b/drivers/usb/cdns3/cdnsp-gadget.h
index f740fa6089d85061e9cd6c46b4ee776cc39bfbbe..e1b5801fdddf8c5cb76d081f3fb1cfb505ffa36e 100644 (file)
--- a/drivers/usb/cdns3/cdnsp-gadget.h
+++ b/drivers/usb/cdns3/cdnsp-gadget.h
@@ -1532,8 +1532,8 @@ void cdnsp_queue_stop_endpoint(struct cdnsp_device *pdev,
                               unsigned int ep_index);
 int cdnsp_queue_ctrl_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq);
 int cdnsp_queue_bulk_tx(struct cdnsp_device *pdev, struct cdnsp_request *preq);
-int cdnsp_queue_isoc_tx_prepare(struct cdnsp_device *pdev,
-                               struct cdnsp_request *preq);
+int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
+                       struct cdnsp_request *preq);
 void cdnsp_queue_configure_endpoint(struct cdnsp_device *pdev,
                                    dma_addr_t in_ctx_ptr);
 void cdnsp_queue_reset_ep(struct cdnsp_device *pdev, unsigned int ep_index);

diff --git a/drivers/usb/cdns3/cdnsp-ring.c b/drivers/usb/cdns3/cdnsp-ring.c
index b23e543b3a3d5ba22871cf17bf4a8f2e43ced3ac..07f6068342d4604720cdf79d849919d0f34e6b97 100644 (file)
--- a/drivers/usb/cdns3/cdnsp-ring.c
+++ b/drivers/usb/cdns3/cdnsp-ring.c
@@ -1333,6 +1333,20 @@ static int cdnsp_handle_tx_event(struct cdnsp_device *pdev,
                                         ep_ring->dequeue, td->last_trb,
                                         ep_trb_dma);
 
+               desc = td->preq->pep->endpoint.desc;
+
+               if (ep_seg) {
+                       ep_trb = &ep_seg->trbs[(ep_trb_dma - ep_seg->dma)
+                                              / sizeof(*ep_trb)];
+
+                       trace_cdnsp_handle_transfer(ep_ring,
+                                       (struct cdnsp_generic_trb *)ep_trb);
+
+                       if (pep->skip && usb_endpoint_xfer_isoc(desc) &&
+                           td->last_trb != ep_trb)
+                               return -EAGAIN;
+               }
+
                /*
                 * Skip the Force Stopped Event. The event_trb(ep_trb_dma)
                 * of FSE is not in the current TD pointed by ep_ring->dequeue
@@ -1347,7 +1361,6 @@ static int cdnsp_handle_tx_event(struct cdnsp_device *pdev,
                        goto cleanup;
                }
 
-               desc = td->preq->pep->endpoint.desc;
                if (!ep_seg) {
                        if (!pep->skip || !usb_endpoint_xfer_isoc(desc)) {
                                /* Something is busted, give up! */
@@ -1374,12 +1387,6 @@ static int cdnsp_handle_tx_event(struct cdnsp_device *pdev,
                        goto cleanup;
                }
 
-               ep_trb = &ep_seg->trbs[(ep_trb_dma - ep_seg->dma)
-                                      / sizeof(*ep_trb)];
-
-               trace_cdnsp_handle_transfer(ep_ring,
-                                           (struct cdnsp_generic_trb *)ep_trb);
-
                if (cdnsp_trb_is_noop(ep_trb))
                        goto cleanup;
 
@@ -1726,11 +1733,6 @@ static unsigned int count_sg_trbs_needed(struct cdnsp_request *preq)
        return num_trbs;
 }
 
-static unsigned int count_isoc_trbs_needed(struct cdnsp_request *preq)
-{
-       return cdnsp_count_trbs(preq->request.dma, preq->request.length);
-}
-
 static void cdnsp_check_trb_math(struct cdnsp_request *preq, int running_total)
 {
        if (running_total != preq->request.length)
@@ -2192,28 +2194,48 @@ static unsigned int
 }
 
 /* Queue function isoc transfer */
-static int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
-                              struct cdnsp_request *preq)
+int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
+                       struct cdnsp_request *preq)
 {
-       int trb_buff_len, td_len, td_remain_len, ret;
+       unsigned int trb_buff_len, td_len, td_remain_len, block_len;
        unsigned int burst_count, last_burst_pkt;
        unsigned int total_pkt_count, max_pkt;
        struct cdnsp_generic_trb *start_trb;
+       struct scatterlist *sg = NULL;
        bool more_trbs_coming = true;
        struct cdnsp_ring *ep_ring;
+       unsigned int num_sgs = 0;
        int running_total = 0;
        u32 field, length_field;
+       u64 addr, send_addr;
        int start_cycle;
        int trbs_per_td;
-       u64 addr;
-       int i;
+       int i, sent_len, ret;
 
        ep_ring = preq->pep->ring;
+
+       td_len = preq->request.length;
+
+       if (preq->request.num_sgs) {
+               num_sgs = preq->request.num_sgs;
+               sg = preq->request.sg;
+               addr = (u64)sg_dma_address(sg);
+               block_len = sg_dma_len(sg);
+               trbs_per_td = count_sg_trbs_needed(preq);
+       } else {
+               addr = (u64)preq->request.dma;
+               block_len = td_len;
+               trbs_per_td = count_trbs_needed(preq);
+       }
+
+       ret = cdnsp_prepare_transfer(pdev, preq, trbs_per_td);
+       if (ret)
+               return ret;
+
        start_trb = &ep_ring->enqueue->generic;
        start_cycle = ep_ring->cycle_state;
-       td_len = preq->request.length;
-       addr = (u64)preq->request.dma;
        td_remain_len = td_len;
+       send_addr = addr;
 
        max_pkt = usb_endpoint_maxp(preq->pep->endpoint.desc);
        total_pkt_count = DIV_ROUND_UP(td_len, max_pkt);
@@ -2225,11 +2247,6 @@ static int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
        burst_count = cdnsp_get_burst_count(pdev, preq, total_pkt_count);
        last_burst_pkt = cdnsp_get_last_burst_packet_count(pdev, preq,
                                                           total_pkt_count);
-       trbs_per_td = count_isoc_trbs_needed(preq);
-
-       ret = cdnsp_prepare_transfer(pdev, preq, trbs_per_td);
-       if (ret)
-               goto cleanup;
 
        /*
         * Set isoc specific data for the first TRB in a TD.
@@ -2248,6 +2265,7 @@ static int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
 
                /* Calculate TRB length. */
                trb_buff_len = TRB_BUFF_LEN_UP_TO_BOUNDARY(addr);
+               trb_buff_len = min(trb_buff_len, block_len);
                if (trb_buff_len > td_remain_len)
                        trb_buff_len = td_remain_len;
 
@@ -2256,7 +2274,8 @@ static int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
                                               trb_buff_len, td_len, preq,
                                               more_trbs_coming, 0);
 
-               length_field = TRB_LEN(trb_buff_len) | TRB_INTR_TARGET(0);
+               length_field = TRB_LEN(trb_buff_len) | TRB_TD_SIZE(remainder) |
+                       TRB_INTR_TARGET(0);
 
                /* Only first TRB is isoc, overwrite otherwise. */
                if (i) {
@@ -2281,12 +2300,27 @@ static int cdnsp_queue_isoc_tx(struct cdnsp_device *pdev,
                }
 
                cdnsp_queue_trb(pdev, ep_ring, more_trbs_coming,
-                               lower_32_bits(addr), upper_32_bits(addr),
+                               lower_32_bits(send_addr), upper_32_bits(send_addr),
                                length_field, field);
 
                running_total += trb_buff_len;
                addr += trb_buff_len;
                td_remain_len -= trb_buff_len;
+
+               sent_len = trb_buff_len;
+               while (sg && sent_len >= block_len) {
+                       /* New sg entry */
+                       --num_sgs;
+                       sent_len -= block_len;
+                       if (num_sgs != 0) {
+                               sg = sg_next(sg);
+                               block_len = sg_dma_len(sg);
+                               addr = (u64)sg_dma_address(sg);
+                               addr += sent_len;
+                       }
+               }
+               block_len -= sent_len;
+               send_addr = addr;
        }
 
        /* Check TD length */
@@ -2324,30 +2358,6 @@ cleanup:
        return ret;
 }
 
-int cdnsp_queue_isoc_tx_prepare(struct cdnsp_device *pdev,
-                               struct cdnsp_request *preq)
-{
-       struct cdnsp_ring *ep_ring;
-       u32 ep_state;
-       int num_trbs;
-       int ret;
-
-       ep_ring = preq->pep->ring;
-       ep_state = GET_EP_CTX_STATE(preq->pep->out_ctx);
-       num_trbs = count_isoc_trbs_needed(preq);
-
-       /*
-        * Check the ring to guarantee there is enough room for the whole
-        * request. Do not insert any td of the USB Request to the ring if the
-        * check failed.
-        */
-       ret = cdnsp_prepare_ring(pdev, ep_ring, ep_state, num_trbs, GFP_ATOMIC);
-       if (ret)
-               return ret;
-
-       return cdnsp_queue_isoc_tx(pdev, preq);
-}
-
 /****          Command Ring Operations         ****/
 /*
  * Generic function for queuing a command TRB on the command ring.