From 435c5408a1ca05ce6c5db234061eb015f6e3cb7c Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@openwrt.org>
Date: Mon, 1 Feb 2016 22:15:05 +0100
Subject: [PATCH] mt76x2: implement fragmented rx support

Signed-off-by: Felix Fietkau <nbd@openwrt.org>
---
 mt76x2.h     |  1 +
 mt76x2_dma.c | 42 +++++++++++++++++++++++++++++++++++-------
 mt76x2_mac.c | 11 ++++++++++-
 3 files changed, 46 insertions(+), 8 deletions(-)

diff --git a/mt76x2.h b/mt76x2.h
index 6325abc0..6476dbb9 100644
--- a/mt76x2.h
+++ b/mt76x2.h
@@ -104,6 +104,7 @@ struct mt76x2_dev {
 	DECLARE_KFIFO_PTR(txstatus_fifo, struct mt76x2_tx_status);
 
 	struct mt76x2_mcu mcu;
+	struct sk_buff *rx_head;
 	struct mt76_queue q_rx;
 
 	struct net_device napi_dev;
diff --git a/mt76x2_dma.c b/mt76x2_dma.c
index 3f5641c2..ffb82025 100644
--- a/mt76x2_dma.c
+++ b/mt76x2_dma.c
@@ -124,16 +124,11 @@ mt76x2_init_tx_queue(struct mt76x2_dev *dev, struct mt76_queue *q,
 
 static void
 mt76x2_process_rx_skb(struct mt76x2_dev *dev, struct mt76_queue *q,
-		    struct sk_buff *skb, u32 info)
+		    struct sk_buff *skb)
 {
 	void *rxwi = skb->data;
 
 	if (q == &dev->mcu.q_rx) {
-		u32 *rxfce;
-
-		rxfce = (u32 *) skb->cb;
-		*rxfce = info;
-
 		skb_queue_tail(&dev->mcu.res_q, skb);
 		wake_up(&dev->mcu.wait);
 		return;
@@ -148,6 +143,23 @@ mt76x2_process_rx_skb(struct mt76x2_dev *dev, struct mt76_queue *q,
 	mt76x2_rx(dev, skb);
 }
 
+static void
+mt76x2_add_fragment(struct mt76x2_dev *dev, struct mt76_queue *q, void *data,
+		    int len, bool more)
+{
+	struct page *page = virt_to_head_page(data);
+	int offset = data - page_address(page);
+	struct sk_buff *skb = q->rx_head;
+
+	skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page, offset, len,
+			q->buf_size);
+
+	if (!more) {
+		q->rx_head = NULL;
+		mt76x2_process_rx_skb(dev, q, skb);
+	}
+}
+
 static int
 mt76x2_process_rx_queue(struct mt76x2_dev *dev, struct mt76_queue *q, int budget)
 {
@@ -165,6 +177,11 @@ mt76x2_process_rx_queue(struct mt76x2_dev *dev, struct mt76_queue *q, int budget
 		if (!data)
 			break;
 
+		if (q->rx_head) {
+			mt76x2_add_fragment(dev, q, data, len, more);
+			continue;
+		}
+
 		skb = build_skb(data, q->buf_size);
 		if (!skb) {
 			skb_free_frag(data);
@@ -177,9 +194,20 @@ mt76x2_process_rx_queue(struct mt76x2_dev *dev, struct mt76_queue *q, int budget
 			continue;
 		}
 
+		if (q == &dev->mcu.q_rx) {
+			u32 * rxfce = (u32 *) skb->cb;
+			*rxfce = info;
+		}
+
 		__skb_put(skb, len);
-		mt76x2_process_rx_skb(dev, q, skb, info);
 		done++;
+
+		if (more) {
+			q->rx_head = skb;
+			continue;
+		}
+
+		mt76x2_process_rx_skb(dev, q, skb);
 	}
 
 	mt76_queue_rx_fill(dev, q, napi);
diff --git a/mt76x2_mac.c b/mt76x2_mac.c
index c006937b..e7db09f8 100644
--- a/mt76x2_mac.c
+++ b/mt76x2_mac.c
@@ -233,7 +233,16 @@ int mt76x2_mac_process_rx(struct mt76x2_dev *dev, struct sk_buff *skb, void *rxi
 	}
 
 	len = MT76_GET(MT_RXWI_CTL_MPDU_LEN, ctl);
-	skb_trim(skb, len);
+	if (WARN_ON_ONCE(len > skb->len))
+		return -EINVAL;
+
+	if (skb_is_nonlinear(skb)) {
+		struct skb_shared_info *sh = skb_shinfo(skb);
+		sh->frags[sh->nr_frags].size -= skb->len - len;
+		skb->len -= 2;
+	} else {
+		skb_trim(skb, len);
+	}
 
 	status->chains = BIT(0) | BIT(1);
 	status->chain_signal[0] = mt76x2_phy_get_rssi(dev, rxwi->rssi[0], 0);