From dd8785a97a9640c3d097af096780af593af31ee7 Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@nbd.name>
Date: Sat, 1 Oct 2022 23:06:59 +0200
Subject: [PATCH] wifi: mt76: dma: add support for mapping skb fragments

Only supported for skbs without frag_list. This is needed to support
scatter/gather for packets on WLAN devices with A-MSDU offload

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 dma.c | 41 ++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 40 insertions(+), 1 deletion(-)

diff --git a/dma.c b/dma.c
index 4b181305..2dd36d61 100644
--- a/dma.c
+++ b/dma.c
@@ -359,12 +359,35 @@ mt76_dma_tx_queue_skb(struct mt76_dev *dev, struct mt76_queue *q,
 		.skb = skb,
 	};
 	struct ieee80211_hw *hw;
-	int len, n = 0, ret = -ENOMEM;
+	int len, i, n = 0, ret = -ENOMEM;
 	struct mt76_txwi_cache *t;
 	struct sk_buff *iter;
 	dma_addr_t addr;
 	u8 *txwi;
 
+	if (skb_shinfo(skb)->nr_frags && skb_shinfo(skb)->frag_list &&
+	    skb_linearize(skb))
+		goto free_skb;
+
+	len = 0;
+	n = skb_shinfo(skb)->nr_frags + 1 - dev->hw->max_tx_fragments;
+	for (i = 0, len = 0; i < n; i++) {
+		skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
+		len += skb_frag_size(frag);
+	}
+	n = 0;
+
+	if (len > 0 && !__pskb_pull_tail(skb, len))
+	    goto free_skb;
+
+	skb_walk_frags(skb, iter) {
+		if (!skb_shinfo(iter)->nr_frags)
+			continue;
+		if (skb_linearize(skb))
+			goto free_skb;
+		break;
+	}
+
 	t = mt76_get_txwi(dev);
 	if (!t)
 		goto free_skb;
@@ -385,6 +408,22 @@ mt76_dma_tx_queue_skb(struct mt76_dev *dev, struct mt76_queue *q,
 	tx_info.buf[n].addr = addr;
 	tx_info.buf[n++].len = len;
 
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
+
+		if (n == ARRAY_SIZE(tx_info.buf))
+			goto unmap;
+
+		len = skb_frag_size(frag);
+		addr = skb_frag_dma_map(dev->dma_dev, frag, 0, len,
+					DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(dev->dma_dev, addr)))
+			goto unmap;
+
+		tx_info.buf[n].addr = addr;
+		tx_info.buf[n++].len = len;
+	}
+
 	skb_walk_frags(skb, iter) {
 		if (n == ARRAY_SIZE(tx_info.buf))
 			goto unmap;