[PATCH 2/5] wifi: mt76: usb: support out-of-order RX URB completion

[Sean Wang <sean.wang@kernel.org>]

From: Sean Wang <sean.wang@mediatek.com>

Keep per-URB RX queue context and complete entries by their real queue
position instead of assuming the completed URB is always at q->head.

USB RX URBs can complete out of order, and advancing q->head too early
can corrupt RX queue accounting and process buffers in the wrong order.

Signed-off-by: Sean Wang <sean.wang@mediatek.com>
---
 drivers/net/wireless/mediatek/mt76/mt76.h |  5 ++
 drivers/net/wireless/mediatek/mt76/usb.c  | 77 ++++++++++++++++-------
 2 files changed, 61 insertions(+), 21 deletions(-)

diff --git a/drivers/net/wireless/mediatek/mt76/mt76.h b/drivers/net/wireless/mediatek/mt76/mt76.h
index 122e77a5f2f4..81740aa7df71 100644
--- a/drivers/net/wireless/mediatek/mt76/mt76.h
+++ b/drivers/net/wireless/mediatek/mt76/mt76.h
@@ -655,6 +655,11 @@ struct mt76_mcu {
 	wait_queue_head_t wait;
 };
 
+struct mt76u_rx_entry {
+	struct mt76_queue_entry *e;
+	struct mt76_queue *q;
+};
+
 #define MT_TX_SG_MAX_SIZE	8
 #define MT_RX_SG_MAX_SIZE	4
 #define MT_NUM_TX_ENTRIES	256
diff --git a/drivers/net/wireless/mediatek/mt76/usb.c b/drivers/net/wireless/mediatek/mt76/usb.c
index ce68e1d0c786..cab36630c978 100644
--- a/drivers/net/wireless/mediatek/mt76/usb.c
+++ b/drivers/net/wireless/mediatek/mt76/usb.c
@@ -397,11 +397,25 @@ mt76u_urb_alloc(struct mt76_dev *dev, struct mt76_queue_entry *e,
 	return 0;
 }
 
+static void mt76u_urb_free(struct urb *urb)
+{
+	int i;
+
+	for (i = 0; i < urb->num_sgs; i++)
+		mt76_put_page_pool_buf(sg_virt(&urb->sg[i]), false);
+
+	if (urb->transfer_buffer)
+		mt76_put_page_pool_buf(urb->transfer_buffer, false);
+
+	usb_free_urb(urb);
+}
+
 static int
 mt76u_rx_urb_alloc(struct mt76_dev *dev, struct mt76_queue *q,
 		   struct mt76_queue_entry *e)
 {
 	enum mt76_rxq_id qid = q - &dev->q_rx[MT_RXQ_MAIN];
+	struct mt76u_rx_entry *rxe;
 	int err, sg_size;
 
 	sg_size = qid == MT_RXQ_MAIN ? MT_RX_SG_MAX_SIZE : 0;
@@ -409,20 +423,25 @@ mt76u_rx_urb_alloc(struct mt76_dev *dev, struct mt76_queue *q,
 	if (err)
 		return err;
 
-	return mt76u_refill_rx(dev, q, e->urb, sg_size);
-}
-
-static void mt76u_urb_free(struct urb *urb)
-{
-	int i;
+	rxe = kzalloc_obj(*rxe, GFP_KERNEL);
+	if (!rxe) {
+		usb_free_urb(e->urb);
+		e->urb = NULL;
+		return -ENOMEM;
+	}
 
-	for (i = 0; i < urb->num_sgs; i++)
-		mt76_put_page_pool_buf(sg_virt(&urb->sg[i]), false);
+	rxe->e = e;
+	rxe->q = q;
+	e->urb->context = rxe;
 
-	if (urb->transfer_buffer)
-		mt76_put_page_pool_buf(urb->transfer_buffer, false);
+	err = mt76u_refill_rx(dev, q, e->urb, sg_size);
+	if (err) {
+		kfree(rxe);
+		mt76u_urb_free(e->urb);
+		e->urb = NULL;
+	}
 
-	usb_free_urb(urb);
+	return err;
 }
 
 static void
@@ -566,8 +585,12 @@ mt76u_process_rx_entry(struct mt76_dev *dev, struct urb *urb,
 static void mt76u_complete_rx(struct urb *urb)
 {
 	struct mt76_dev *dev = dev_get_drvdata(&urb->dev->dev);
-	struct mt76_queue *q = urb->context;
+	struct mt76u_rx_entry *rxe = urb->context;
+	struct mt76_queue_entry *e = rxe->e;
+	unsigned int idx, pending, pos;
+	struct mt76_queue *q = rxe->q;
 	unsigned long flags;
+	bool wake = false;
 
 	trace_rx_urb(dev, urb);
 
@@ -586,18 +609,28 @@ static void mt76u_complete_rx(struct urb *urb)
 	}
 
 	spin_lock_irqsave(&q->lock, flags);
-	if (WARN_ONCE(q->entry[q->head].urb != urb, "rx urb mismatch"))
+	idx = e - q->entry;
+	pending = q->ndesc - q->queued;
+	pos = (idx + q->ndesc - q->head) % q->ndesc;
+	if (WARN_ONCE(idx >= q->ndesc || pos >= pending, "rx urb mismatch"))
 		goto out;
 
-	q->head = (q->head + 1) % q->ndesc;
-	q->queued++;
-
-	if (q == &dev->q_rx[MT_RXQ_MAIN])
-		napi_schedule(&dev->napi[MT_RXQ_MAIN]);
-	else
-		mt76_worker_schedule(&dev->usb.rx_worker);
+	e->done = true;
+	while (q->entry[q->head].done) {
+		q->entry[q->head].done = false;
+		q->head = (q->head + 1) % q->ndesc;
+		q->queued++;
+		wake = true;
+	}
 out:
 	spin_unlock_irqrestore(&q->lock, flags);
+
+	if (wake) {
+		if (q == &dev->q_rx[MT_RXQ_MAIN])
+			napi_schedule(&dev->napi[MT_RXQ_MAIN]);
+		else
+			mt76_worker_schedule(&dev->usb.rx_worker);
+	}
 }
 
 static int
@@ -607,7 +640,7 @@ mt76u_submit_rx_buf(struct mt76_dev *dev, enum mt76_rxq_id qid,
 	int ep = qid == MT_RXQ_MAIN ? MT_EP_IN_PKT_RX : MT_EP_IN_CMD_RESP;
 
 	mt76u_fill_bulk_urb(dev, USB_DIR_IN, ep, urb,
-			    mt76u_complete_rx, &dev->q_rx[qid]);
+			    mt76u_complete_rx, urb->context);
 	trace_submit_urb(dev, urb);
 
 	return usb_submit_urb(urb, GFP_ATOMIC);
@@ -678,6 +711,7 @@ mt76u_submit_rx_buffers(struct mt76_dev *dev, enum mt76_rxq_id qid)
 
 	spin_lock_irqsave(&q->lock, flags);
 	for (i = 0; i < q->ndesc; i++) {
+		q->entry[i].done = false;
 		err = mt76u_submit_rx_buf(dev, qid, q->entry[i].urb);
 		if (err < 0)
 			break;
@@ -733,6 +767,7 @@ mt76u_free_rx_queue(struct mt76_dev *dev, struct mt76_queue *q)
 		if (!q->entry[i].urb)
 			continue;
 
+		kfree(q->entry[i].urb->context);
 		mt76u_urb_free(q->entry[i].urb);
 		q->entry[i].urb = NULL;
 	}
-- 
2.43.0