From 82b0417e4bfc5af8c1710996913374f09f4a2a03 Mon Sep 17 00:00:00 2001 From: Pawel Laszczak Date: Thu, 22 Dec 2022 04:09:34 -0500 Subject: 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 Reviewed-by: Peter Chen Link: https://lore.kernel.org/r/20221222090934.145140-1-pawell@cadence.com Signed-off-by: Greg Kroah-Hartman --- drivers/usb/cdns3/cdnsp-gadget.c | 2 +- drivers/usb/cdns3/cdnsp-gadget.h | 4 +- drivers/usb/cdns3/cdnsp-ring.c | 110 +++++++++++++++++++++------------------ 3 files changed, 63 insertions(+), 53 deletions(-) diff --git a/drivers/usb/cdns3/cdnsp-gadget.c b/drivers/usb/cdns3/cdnsp-gadget.c index f9aa50ff14d4..fff9ec9c391f 100644 --- 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 f740fa6089d8..e1b5801fdddf 100644 --- 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 b23e543b3a3d..07f6068342d4 100644 --- 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. -- cgit v1.2.3