net: mhi-net: Add de-aggeration support

When device side MTU is larger than host side MTU, the packets
(typically rmnet packets) are split over multiple MHI transfers.
In that case, fragments must be re-aggregated to recover the packet
before forwarding to upper layer.

A fragmented packet result in -EOVERFLOW MHI transaction status for
each of its fragments, except the final one. Such transfer was
previoulsy considered as error and fragments were simply dropped.

This patch implements the aggregation mechanism allowing to recover
the initial packet. It also print a warning (once) since this behavior
usually comes from a misconfiguration of the device (modem).

Signed-off-by: Loic Poulain <loic.poulain@linaro.org>

1 files changed, 64 insertions, 10 deletions

diff --git a/drivers/net/mhi_net.c b/drivers/net/mhi_net.c
index 478e78f17caa..a833850be9ab 100644
--- a/drivers/net/mhi_net.c
+++ b/drivers/net/mhi_net.c
@@ -33,6 +33,7 @@ struct mhi_net_dev {
 	struct mhi_device *mdev;
 	struct net_device *ndev;
 	struct delayed_work rx_refill;
+	struct sk_buff *skbagg;
 	struct mhi_net_stats stats;
 	u32 rx_queue_sz;
 };
@@ -132,6 +133,31 @@ static void mhi_net_setup(struct net_device *ndev)
 	ndev->tx_queue_len = 1000;
 }
 
+static struct sk_buff *mhi_net_skb_append(struct mhi_device *mhi_dev,
+					  struct sk_buff *skb1,
+					  struct sk_buff *skb2)
+{
+	struct sk_buff *new_skb;
+
+	/* This is the first fragment */
+	if (!skb1)
+		return skb2;
+
+	/* Expand packet */
+	new_skb = skb_copy_expand(skb1, 0, skb2->len, GFP_ATOMIC);
+	dev_kfree_skb_any(skb1);
+	if (!new_skb)
+		return skb2;
+
+	/* Append to expanded packet */
+	memcpy(skb_put(new_skb, skb2->len), skb2->data, skb2->len);
+
+	/* free appended skb */
+	dev_kfree_skb_any(skb2);
+
+	return new_skb;
+}
+
 static void mhi_net_dl_callback(struct mhi_device *mhi_dev,
 				struct mhi_result *mhi_res)
 {
@@ -142,19 +168,44 @@ static void mhi_net_dl_callback(struct mhi_device *mhi_dev,
 	free_desc_count = mhi_get_free_desc_count(mhi_dev, DMA_FROM_DEVICE);
 
 	if (unlikely(mhi_res->transaction_status)) {
-		dev_kfree_skb_any(skb);
-
-		/* MHI layer stopping/resetting the DL channel */
-		if (mhi_res->transaction_status == -ENOTCONN)
+		switch (mhi_res->transaction_status) {
+		case -EOVERFLOW:
+			/* Packet can not fit in one MHI buffer and has been
+			 * split over multiple MHI transfers, do re-aggregation.
+			 * That usually means the device side MTU is larger than
+			 * the host side MTU/MRU. Since this is not optimal,
+			 * print a warning (once).
+			 */
+			netdev_warn_once(mhi_netdev->ndev,
+					 "Fragmented packets received, fix MTU?\n");
+			skb_put(skb, mhi_res->bytes_xferd);
+			mhi_netdev->skbagg = mhi_net_skb_append(mhi_dev,
+								mhi_netdev->skbagg,
+								skb);
+			break;
+		case -ENOTCONN:
+			/* MHI layer stopping/resetting the DL channel */
+			dev_kfree_skb_any(skb);
 			return;
-
-		u64_stats_update_begin(&mhi_netdev->stats.rx_syncp);
-		u64_stats_inc(&mhi_netdev->stats.rx_errors);
-		u64_stats_update_end(&mhi_netdev->stats.rx_syncp);
+		default:
+			/* Unknown error, simply drop */
+			dev_kfree_skb_any(skb);
+			u64_stats_update_begin(&mhi_netdev->stats.rx_syncp);
+			u64_stats_inc(&mhi_netdev->stats.rx_errors);
+			u64_stats_update_end(&mhi_netdev->stats.rx_syncp);
+		}
 	} else {
+		skb_put(skb, mhi_res->bytes_xferd);
+
+		if (unlikely(mhi_netdev->skbagg)) {
+			/* Aggregate the final fragment */
+			skb = mhi_net_skb_append(mhi_dev, mhi_netdev->skbagg, skb);
+			mhi_netdev->skbagg = NULL;
+		}
+
 		u64_stats_update_begin(&mhi_netdev->stats.rx_syncp);
 		u64_stats_inc(&mhi_netdev->stats.rx_packets);
-		u64_stats_add(&mhi_netdev->stats.rx_bytes, mhi_res->bytes_xferd);
+		u64_stats_add(&mhi_netdev->stats.rx_bytes, skb->len);
 		u64_stats_update_end(&mhi_netdev->stats.rx_syncp);
 
 		switch (skb->data[0] & 0xf0) {
@@ -169,7 +220,6 @@ static void mhi_net_dl_callback(struct mhi_device *mhi_dev,
 			break;
 		}
 
-		skb_put(skb, mhi_res->bytes_xferd);
 		netif_rx(skb);
 	}
 
@@ -267,6 +317,7 @@ static int mhi_net_probe(struct mhi_device *mhi_dev,
 	dev_set_drvdata(dev, mhi_netdev);
 	mhi_netdev->ndev = ndev;
 	mhi_netdev->mdev = mhi_dev;
+	mhi_netdev->skbagg = NULL;
 	SET_NETDEV_DEV(ndev, &mhi_dev->dev);
 	SET_NETDEV_DEVTYPE(ndev, &wwan_type);
 
@@ -301,6 +352,9 @@ static void mhi_net_remove(struct mhi_device *mhi_dev)
 
 	mhi_unprepare_from_transfer(mhi_netdev->mdev);
 
+	if (mhi_netdev->skbagg)
+		kfree_skb(mhi_netdev->skbagg);
+
 	free_netdev(mhi_netdev->ndev);
 }