From a957f7480eb2328aad46a58630db55149a60ffbe Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@openwrt.org>
Date: Sun, 28 Feb 2016 19:03:45 +0100
Subject: [PATCH] dma: add function for resetting the state of an rx queue

Signed-off-by: Felix Fietkau <nbd@openwrt.org>
---
 dma.c  | 32 +++++++++++++++++++++++++++-----
 mt76.h |  3 +++
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/dma.c b/dma.c
index 16dd5f3b..2d23467b 100644
--- a/dma.c
+++ b/dma.c
@@ -122,6 +122,13 @@ mt76_dma_tx_cleanup_idx(struct mt76_dev *dev, struct mt76_queue *q, int idx,
 	memset(e, 0, sizeof(*e));
 }
 
+static void
+mt76_dma_sync_idx(struct mt76_dev *dev, struct mt76_queue *q)
+{
+	q->tail = q->head = ioread32(&q->regs->dma_idx);
+	iowrite32(q->head, &q->regs->cpu_idx);
+}
+
 static void
 mt76_dma_tx_cleanup(struct mt76_dev *dev, enum mt76_txq_id qid, bool flush)
 {
@@ -155,12 +162,12 @@ mt76_dma_tx_cleanup(struct mt76_dev *dev, enum mt76_txq_id qid, bool flush)
 		if (!flush && q->tail == last)
 		    last = ioread32(&q->regs->dma_idx);
 	}
-	if (!flush) {
+
+	if (!flush)
 		mt76_txq_schedule(dev, q);
-	} else {
-		iowrite32(q->tail, &q->regs->dma_idx);
-		iowrite32(q->head, &q->regs->cpu_idx);
-	}
+	else
+		mt76_dma_sync_idx(dev, q);
+
 	spin_unlock_bh(&q->lock);
 }
 
@@ -277,6 +284,20 @@ mt76_dma_rx_cleanup(struct mt76_dev *dev, struct mt76_queue *q)
 	spin_unlock_bh(&q->lock);
 }
 
+static void
+mt76_dma_rx_reset(struct mt76_dev *dev, enum mt76_rxq_id qid)
+{
+	struct mt76_queue *q = &dev->q_rx[qid];
+	int i;
+
+	for (i = 0; i < q->ndesc; i++)
+		q->desc[i].ctrl &= ~cpu_to_le32(MT_DMA_CTL_DMA_DONE);
+
+	mt76_dma_rx_cleanup(dev, q);
+	mt76_dma_sync_idx(dev, q);
+	mt76_dma_rx_fill(dev, q, false);
+}
+
 static void
 mt76_add_fragment(struct mt76_dev *dev, struct mt76_queue *q, void *data,
 		    int len, bool more)
@@ -390,6 +411,7 @@ static const struct mt76_queue_ops mt76_dma_ops = {
 	.alloc = mt76_dma_alloc_queue,
 	.add_buf = mt76_dma_add_buf,
 	.tx_cleanup = mt76_dma_tx_cleanup,
+	.rx_reset = mt76_dma_rx_reset,
 	.kick = mt76_dma_kick_queue,
 };
 
diff --git a/mt76.h b/mt76.h
index 0990a054..33e4e3e6 100644
--- a/mt76.h
+++ b/mt76.h
@@ -109,6 +109,8 @@ struct mt76_queue_ops {
 	void *(*dequeue)(struct mt76_dev *dev, struct mt76_queue *q, bool flush,
 			 int *len, u32 *info, bool *more);
 
+	void (*rx_reset)(struct mt76_dev *dev, enum mt76_rxq_id qid);
+
 	void (*tx_cleanup)(struct mt76_dev *dev, enum mt76_txq_id qid, bool flush);
 
 	void (*kick)(struct mt76_dev *dev, struct mt76_queue *q);
@@ -247,6 +249,7 @@ static inline u16 mt76_rev(struct mt76_dev *dev)
 #define mt76_init_queues(dev)		(dev)->mt76.queue_ops->init(&((dev)->mt76))
 #define mt76_queue_alloc(dev, ...)	(dev)->mt76.queue_ops->alloc(&((dev)->mt76), __VA_ARGS__)
 #define mt76_queue_add_buf(dev, ...)	(dev)->mt76.queue_ops->add_buf(&((dev)->mt76), __VA_ARGS__)
+#define mt76_queue_rx_reset(dev, ...)	(dev)->mt76.queue_ops->rx_reset(&((dev)->mt76), __VA_ARGS__)
 #define mt76_queue_tx_cleanup(dev, ...)	(dev)->mt76.queue_ops->tx_cleanup(&((dev)->mt76), __VA_ARGS__)
 #define mt76_queue_kick(dev, ...)	(dev)->mt76.queue_ops->kick(&((dev)->mt76), __VA_ARGS__)