Merge branch 'Add-MBIM-over-MHI-support'

Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/Makefile

@@ -36,7 +36,7 @@ obj-$(CONFIG_GTP) += gtp.o
 obj-$(CONFIG_NLMON) += nlmon.o
 obj-$(CONFIG_NET_VRF) += vrf.o
 obj-$(CONFIG_VSOCKMON) += vsockmon.o
-obj-$(CONFIG_MHI_NET) += mhi_net.o
+obj-$(CONFIG_MHI_NET) += mhi/
 
 #
 # Networking Drivers

drivers/net/mhi/Makefile

+obj-$(CONFIG_MHI_NET) += mhi_net.o
+
+mhi_net-y := net.o proto_mbim.o

drivers/net/mhi/mhi.h

+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/* MHI Network driver - Network over MHI bus
+ *
+ * Copyright (C) 2021 Linaro Ltd <loic.poulain@linaro.org>
+ */
+
+struct mhi_net_stats {
+	u64_stats_t rx_packets;
+	u64_stats_t rx_bytes;
+	u64_stats_t rx_errors;
+	u64_stats_t rx_dropped;
+	u64_stats_t rx_length_errors;
+	u64_stats_t tx_packets;
+	u64_stats_t tx_bytes;
+	u64_stats_t tx_errors;
+	u64_stats_t tx_dropped;
+	struct u64_stats_sync tx_syncp;
+	struct u64_stats_sync rx_syncp;
+};
+
+struct mhi_net_dev {
+	struct mhi_device *mdev;
+	struct net_device *ndev;
+	struct sk_buff *skbagg_head;
+	struct sk_buff *skbagg_tail;
+	const struct mhi_net_proto *proto;
+	void *proto_data;
+	struct delayed_work rx_refill;
+	struct mhi_net_stats stats;
+	u32 rx_queue_sz;
+	int msg_enable;
+};
+
+struct mhi_net_proto {
+	int (*init)(struct mhi_net_dev *mhi_netdev);
+	struct sk_buff * (*tx_fixup)(struct mhi_net_dev *mhi_netdev, struct sk_buff *skb);
+	void (*rx)(struct mhi_net_dev *mhi_netdev, struct sk_buff *skb);
+};
+
+extern const struct mhi_net_proto proto_mbim;

drivers/net/mhi_net.c → drivers/net/mhi/net.c

@@ -12,31 +12,15 @@
 #include <linux/skbuff.h>
 #include <linux/u64_stats_sync.h>
 
+#include "mhi.h"
+
 #define MHI_NET_MIN_MTU		ETH_MIN_MTU
 #define MHI_NET_MAX_MTU		0xffff
 #define MHI_NET_DEFAULT_MTU	0x4000
 
-struct mhi_net_stats {
-	u64_stats_t rx_packets;
-	u64_stats_t rx_bytes;
-	u64_stats_t rx_errors;
-	u64_stats_t rx_dropped;
-	u64_stats_t tx_packets;
-	u64_stats_t tx_bytes;
-	u64_stats_t tx_errors;
-	u64_stats_t tx_dropped;
-	struct u64_stats_sync tx_syncp;
-	struct u64_stats_sync rx_syncp;
-};
-
-struct mhi_net_dev {
-	struct mhi_device *mdev;
-	struct net_device *ndev;
-	struct sk_buff *skbagg_head;
-	struct sk_buff *skbagg_tail;
-	struct delayed_work rx_refill;
-	struct mhi_net_stats stats;
-	u32 rx_queue_sz;
+struct mhi_device_info {
+	const char *netname;
+	const struct mhi_net_proto *proto;
 };
 
 static int mhi_ndo_open(struct net_device *ndev)
@@ -68,26 +52,35 @@ static int mhi_ndo_stop(struct net_device *ndev)
 static int mhi_ndo_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct mhi_net_dev *mhi_netdev = netdev_priv(ndev);
+	const struct mhi_net_proto *proto = mhi_netdev->proto;
 	struct mhi_device *mdev = mhi_netdev->mdev;
 	int err;
 
+	if (proto && proto->tx_fixup) {
+		skb = proto->tx_fixup(mhi_netdev, skb);
+		if (unlikely(!skb))
+			goto exit_drop;
+	}
+
 	err = mhi_queue_skb(mdev, DMA_TO_DEVICE, skb, skb->len, MHI_EOT);
 	if (unlikely(err)) {
 		net_err_ratelimited("%s: Failed to queue TX buf (%d)\n",
 				    ndev->name, err);
-
-		u64_stats_update_begin(&mhi_netdev->stats.tx_syncp);
-		u64_stats_inc(&mhi_netdev->stats.tx_dropped);
-		u64_stats_update_end(&mhi_netdev->stats.tx_syncp);
-
-		/* drop the packet */
 		dev_kfree_skb_any(skb);
+		goto exit_drop;
 	}
 
 	if (mhi_queue_is_full(mdev, DMA_TO_DEVICE))
 		netif_stop_queue(ndev);
 
 	return NETDEV_TX_OK;
+
+exit_drop:
+	u64_stats_update_begin(&mhi_netdev->stats.tx_syncp);
+	u64_stats_inc(&mhi_netdev->stats.tx_dropped);
+	u64_stats_update_end(&mhi_netdev->stats.tx_syncp);
+
+	return NETDEV_TX_OK;
 }
 
 static void mhi_ndo_get_stats64(struct net_device *ndev,
@@ -102,6 +95,7 @@ static void mhi_ndo_get_stats64(struct net_device *ndev,
 		stats->rx_bytes = u64_stats_read(&mhi_netdev->stats.rx_bytes);
 		stats->rx_errors = u64_stats_read(&mhi_netdev->stats.rx_errors);
 		stats->rx_dropped = u64_stats_read(&mhi_netdev->stats.rx_dropped);
+		stats->rx_length_errors = u64_stats_read(&mhi_netdev->stats.rx_length_errors);
 	} while (u64_stats_fetch_retry_irq(&mhi_netdev->stats.rx_syncp, start));
 
 	do {
@@ -164,6 +158,7 @@ static void mhi_net_dl_callback(struct mhi_device *mhi_dev,
 				struct mhi_result *mhi_res)
 {
 	struct mhi_net_dev *mhi_netdev = dev_get_drvdata(&mhi_dev->dev);
+	const struct mhi_net_proto *proto = mhi_netdev->proto;
 	struct sk_buff *skb = mhi_res->buf_addr;
 	int free_desc_count;
 
@@ -220,7 +215,10 @@ static void mhi_net_dl_callback(struct mhi_device *mhi_dev,
 			break;
 		}
 
-		netif_rx(skb);
+		if (proto && proto->rx)
+			proto->rx(mhi_netdev, skb);
+		else
+			netif_rx(skb);
 	}
 
 	/* Refill if RX buffers queue becomes low */
@@ -302,14 +300,14 @@ static struct device_type wwan_type = {
 static int mhi_net_probe(struct mhi_device *mhi_dev,
 			 const struct mhi_device_id *id)
 {
-	const char *netname = (char *)id->driver_data;
+	const struct mhi_device_info *info = (struct mhi_device_info *)id->driver_data;
 	struct device *dev = &mhi_dev->dev;
 	struct mhi_net_dev *mhi_netdev;
 	struct net_device *ndev;
 	int err;
 
-	ndev = alloc_netdev(sizeof(*mhi_netdev), netname, NET_NAME_PREDICTABLE,
-			    mhi_net_setup);
+	ndev = alloc_netdev(sizeof(*mhi_netdev), info->netname,
+			    NET_NAME_PREDICTABLE, mhi_net_setup);
 	if (!ndev)
 		return -ENOMEM;
 
@@ -318,6 +316,7 @@ static int mhi_net_probe(struct mhi_device *mhi_dev,
 	mhi_netdev->ndev = ndev;
 	mhi_netdev->mdev = mhi_dev;
 	mhi_netdev->skbagg_head = NULL;
+	mhi_netdev->proto = info->proto;
 	SET_NETDEV_DEV(ndev, &mhi_dev->dev);
 	SET_NETDEV_DEVTYPE(ndev, &wwan_type);
 
@@ -337,8 +336,16 @@ static int mhi_net_probe(struct mhi_device *mhi_dev,
 	if (err)
 		goto out_err;
 
+	if (mhi_netdev->proto) {
+		err = mhi_netdev->proto->init(mhi_netdev);
+		if (err)
+			goto out_err_proto;
+	}
+
 	return 0;
 
+out_err_proto:
+	unregister_netdev(ndev);
 out_err:
 	free_netdev(ndev);
 	return err;
@@ -358,9 +365,26 @@ static void mhi_net_remove(struct mhi_device *mhi_dev)
 	free_netdev(mhi_netdev->ndev);
 }
 
+static const struct mhi_device_info mhi_hwip0 = {
+	.netname = "mhi_hwip%d",
+};
+
+static const struct mhi_device_info mhi_swip0 = {
+	.netname = "mhi_swip%d",
+};
+
+static const struct mhi_device_info mhi_hwip0_mbim = {
+	.netname = "mhi_mbim%d",
+	.proto = &proto_mbim,
+};
+
 static const struct mhi_device_id mhi_net_id_table[] = {
-	{ .chan = "IP_HW0", .driver_data = (kernel_ulong_t)"mhi_hwip%d" },
-	{ .chan = "IP_SW0", .driver_data = (kernel_ulong_t)"mhi_swip%d" },
+	/* Hardware accelerated data PATH (to modem IPA), protocol agnostic */
+	{ .chan = "IP_HW0", .driver_data = (kernel_ulong_t)&mhi_hwip0 },
+	/* Software data PATH (to modem CPU) */
+	{ .chan = "IP_SW0", .driver_data = (kernel_ulong_t)&mhi_swip0 },
+	/* Hardware accelerated data PATH (to modem IPA), MBIM protocol */
+	{ .chan = "IP_HW0_MBIM", .driver_data = (kernel_ulong_t)&mhi_hwip0_mbim },
 	{}
 };
 MODULE_DEVICE_TABLE(mhi, mhi_net_id_table);

drivers/net/mhi/proto_mbim.c

+// SPDX-License-Identifier: GPL-2.0-or-later
+/* MHI Network driver - Network over MHI bus
+ *
+ * Copyright (C) 2021 Linaro Ltd <loic.poulain@linaro.org>
+ *
+ * This driver copy some code from cdc_ncm, which is:
+ * Copyright (C) ST-Ericsson 2010-2012
+ * and cdc_mbim, which is:
+ * Copyright (c) 2012  Smith Micro Software, Inc.
+ * Copyright (c) 2012  Bjørn Mork <bjorn@mork.no>
+ *
+ */
+
+#include <linux/ethtool.h>
+#include <linux/if_vlan.h>
+#include <linux/ip.h>
+#include <linux/mii.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/usb.h>
+#include <linux/usb/cdc.h>
+#include <linux/usb/usbnet.h>
+#include <linux/usb/cdc_ncm.h>
+
+#include "mhi.h"
+
+#define MBIM_NDP16_SIGN_MASK 0x00ffffff
+
+struct mbim_context {
+	u16 rx_seq;
+	u16 tx_seq;
+};
+
+static void __mbim_length_errors_inc(struct mhi_net_dev *dev)
+{
+	u64_stats_update_begin(&dev->stats.rx_syncp);
+	u64_stats_inc(&dev->stats.rx_length_errors);
+	u64_stats_update_end(&dev->stats.rx_syncp);
+}
+
+static void __mbim_errors_inc(struct mhi_net_dev *dev)
+{
+	u64_stats_update_begin(&dev->stats.rx_syncp);
+	u64_stats_inc(&dev->stats.rx_errors);
+	u64_stats_update_end(&dev->stats.rx_syncp);
+}
+
+static int mbim_rx_verify_nth16(struct sk_buff *skb)
+{
+	struct mhi_net_dev *dev = netdev_priv(skb->dev);
+	struct mbim_context *ctx = dev->proto_data;
+	struct usb_cdc_ncm_nth16 *nth16;
+	int len;
+
+	if (skb->len < sizeof(struct usb_cdc_ncm_nth16) +
+			sizeof(struct usb_cdc_ncm_ndp16)) {
+		netif_dbg(dev, rx_err, dev->ndev, "frame too short\n");
+		__mbim_length_errors_inc(dev);
+		return -EINVAL;
+	}
+
+	nth16 = (struct usb_cdc_ncm_nth16 *)skb->data;
+
+	if (nth16->dwSignature != cpu_to_le32(USB_CDC_NCM_NTH16_SIGN)) {
+		netif_dbg(dev, rx_err, dev->ndev,
+			  "invalid NTH16 signature <%#010x>\n",
+			  le32_to_cpu(nth16->dwSignature));
+		__mbim_errors_inc(dev);
+		return -EINVAL;
+	}
+
+	/* No limit on the block length, except the size of the data pkt */
+	len = le16_to_cpu(nth16->wBlockLength);
+	if (len > skb->len) {
+		netif_dbg(dev, rx_err, dev->ndev,
+			  "NTB does not fit into the skb %u/%u\n", len,
+			  skb->len);
+		__mbim_length_errors_inc(dev);
+		return -EINVAL;
+	}
+
+	if (ctx->rx_seq + 1 != le16_to_cpu(nth16->wSequence) &&
+	    (ctx->rx_seq || le16_to_cpu(nth16->wSequence)) &&
+	    !(ctx->rx_seq == 0xffff && !le16_to_cpu(nth16->wSequence))) {
+		netif_dbg(dev, rx_err, dev->ndev,
+			  "sequence number glitch prev=%d curr=%d\n",
+			  ctx->rx_seq, le16_to_cpu(nth16->wSequence));
+	}
+	ctx->rx_seq = le16_to_cpu(nth16->wSequence);
+
+	return le16_to_cpu(nth16->wNdpIndex);
+}
+
+static int mbim_rx_verify_ndp16(struct sk_buff *skb, int ndpoffset)
+{
+	struct mhi_net_dev *dev = netdev_priv(skb->dev);
+	struct usb_cdc_ncm_ndp16 *ndp16;
+	int ret;
+
+	if (ndpoffset + sizeof(struct usb_cdc_ncm_ndp16) > skb->len) {
+		netif_dbg(dev, rx_err, dev->ndev, "invalid NDP offset  <%u>\n",
+			  ndpoffset);
+		return -EINVAL;
+	}
+
+	ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb->data + ndpoffset);
+
+	if (le16_to_cpu(ndp16->wLength) < USB_CDC_NCM_NDP16_LENGTH_MIN) {
+		netif_dbg(dev, rx_err, dev->ndev, "invalid DPT16 length <%u>\n",
+			  le16_to_cpu(ndp16->wLength));
+		return -EINVAL;
+	}
+
+	ret = ((le16_to_cpu(ndp16->wLength) - sizeof(struct usb_cdc_ncm_ndp16))
+			/ sizeof(struct usb_cdc_ncm_dpe16));
+	ret--; /* Last entry is always a NULL terminator */
+
+	if (sizeof(struct usb_cdc_ncm_ndp16) +
+	     ret * sizeof(struct usb_cdc_ncm_dpe16) > skb->len) {
+		netif_dbg(dev, rx_err, dev->ndev,
+			  "Invalid nframes = %d\n", ret);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static void mbim_rx(struct mhi_net_dev *mhi_netdev, struct sk_buff *skb)
+{
+	struct net_device *ndev = mhi_netdev->ndev;
+	int ndpoffset;
+
+	if (skb_linearize(skb))
+		goto error;
+
+	/* Check NTB header and retrieve first NDP offset */
+	ndpoffset = mbim_rx_verify_nth16(skb);
+	if (ndpoffset < 0) {
+		net_err_ratelimited("%s: Incorrect NTB header\n", ndev->name);
+		goto error;
+	}
+
+	/* Process each NDP */
+	while (1) {
+		struct usb_cdc_ncm_ndp16 *ndp16;
+		struct usb_cdc_ncm_dpe16 *dpe16;
+		int nframes, n;
+
+		/* Check NDP header and retrieve number of datagrams */
+		nframes = mbim_rx_verify_ndp16(skb, ndpoffset);
+		if (nframes < 0) {
+			net_err_ratelimited("%s: Incorrect NDP16\n", ndev->name);
+			__mbim_length_errors_inc(mhi_netdev);
+			goto error;
+		}
+
+		 /* Only IP data type supported, no DSS in MHI context */
+		ndp16 = (struct usb_cdc_ncm_ndp16 *)(skb->data + ndpoffset);
+		if ((ndp16->dwSignature & cpu_to_le32(MBIM_NDP16_SIGN_MASK))
+				!= cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN)) {
+			net_err_ratelimited("%s: Unsupported NDP type\n", ndev->name);
+			__mbim_errors_inc(mhi_netdev);
+			goto next_ndp;
+		}
+
+		/* Only primary IP session 0 (0x00) supported for now */
+		if (ndp16->dwSignature & ~cpu_to_le32(MBIM_NDP16_SIGN_MASK)) {
+			net_err_ratelimited("%s: bad packet session\n", ndev->name);
+			__mbim_errors_inc(mhi_netdev);
+			goto next_ndp;
+		}
+
+		/* de-aggregate and deliver IP packets */
+		dpe16 = ndp16->dpe16;
+		for (n = 0; n < nframes; n++, dpe16++) {
+			u16 dgram_offset = le16_to_cpu(dpe16->wDatagramIndex);
+			u16 dgram_len = le16_to_cpu(dpe16->wDatagramLength);
+			struct sk_buff *skbn;
+
+			if (!dgram_offset || !dgram_len)
+				break; /* null terminator */
+
+			skbn = netdev_alloc_skb(ndev, dgram_len);
+			if (!skbn)
+				continue;
+
+			skb_put(skbn, dgram_len);
+			memcpy(skbn->data, skb->data + dgram_offset, dgram_len);
+
+			switch (skbn->data[0] & 0xf0) {
+			case 0x40:
+				skbn->protocol = htons(ETH_P_IP);
+				break;
+			case 0x60:
+				skbn->protocol = htons(ETH_P_IPV6);
+				break;
+			default:
+				net_err_ratelimited("%s: unknown protocol\n",
+						    ndev->name);
+				__mbim_errors_inc(mhi_netdev);
+				dev_kfree_skb_any(skbn);
+				continue;
+			}
+
+			netif_rx(skbn);
+		}
+next_ndp:
+		/* Other NDP to process? */
+		ndpoffset = (int)le16_to_cpu(ndp16->wNextNdpIndex);
+		if (!ndpoffset)
+			break;
+	}
+
+	/* free skb */
+	dev_consume_skb_any(skb);
+	return;
+error:
+	dev_kfree_skb_any(skb);
+}
+
+struct mbim_tx_hdr {
+	struct usb_cdc_ncm_nth16 nth16;
+	struct usb_cdc_ncm_ndp16 ndp16;
+	struct usb_cdc_ncm_dpe16 dpe16[2];
+} __packed;
+
+static struct sk_buff *mbim_tx_fixup(struct mhi_net_dev *mhi_netdev,
+				     struct sk_buff *skb)
+{
+	struct mbim_context *ctx = mhi_netdev->proto_data;
+	unsigned int dgram_size = skb->len;
+	struct usb_cdc_ncm_nth16 *nth16;
+	struct usb_cdc_ncm_ndp16 *ndp16;
+	struct mbim_tx_hdr *mbim_hdr;
+
+	/* For now, this is a partial implementation of CDC MBIM, only one NDP
+	 * is sent, containing the IP packet (no aggregation).
+	 */
+
+	/* Ensure we have enough headroom for crafting MBIM header */
+	if (skb_cow_head(skb, sizeof(struct mbim_tx_hdr))) {
+		dev_kfree_skb_any(skb);
+		return NULL;
+	}
+
+	mbim_hdr = skb_push(skb, sizeof(struct mbim_tx_hdr));
+
+	/* Fill NTB header */
+	nth16 = &mbim_hdr->nth16;
+	nth16->dwSignature = cpu_to_le32(USB_CDC_NCM_NTH16_SIGN);
+	nth16->wHeaderLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
+	nth16->wSequence = cpu_to_le16(ctx->tx_seq++);
+	nth16->wBlockLength = cpu_to_le16(skb->len);
+	nth16->wNdpIndex = cpu_to_le16(sizeof(struct usb_cdc_ncm_nth16));
+
+	/* Fill the unique NDP */
+	ndp16 = &mbim_hdr->ndp16;
+	ndp16->dwSignature = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
+	ndp16->wLength = cpu_to_le16(sizeof(struct usb_cdc_ncm_ndp16)
+					+ sizeof(struct usb_cdc_ncm_dpe16) * 2);
+	ndp16->wNextNdpIndex = 0;
+
+	/* Datagram follows the mbim header */
+	ndp16->dpe16[0].wDatagramIndex = cpu_to_le16(sizeof(struct mbim_tx_hdr));
+	ndp16->dpe16[0].wDatagramLength = cpu_to_le16(dgram_size);
+
+	/* null termination */
+	ndp16->dpe16[1].wDatagramIndex = 0;
+	ndp16->dpe16[1].wDatagramLength = 0;
+
+	return skb;
+}
+
+static int mbim_init(struct mhi_net_dev *mhi_netdev)
+{
+	struct net_device *ndev = mhi_netdev->ndev;
+
+	mhi_netdev->proto_data = devm_kzalloc(&ndev->dev,
+					      sizeof(struct mbim_context),
+					      GFP_KERNEL);
+	if (!mhi_netdev->proto_data)
+		return -ENOMEM;
+
+	ndev->needed_headroom = sizeof(struct mbim_tx_hdr);
+
+	return 0;
+}
+
+const struct mhi_net_proto proto_mbim = {
+	.init = mbim_init,
+	.rx = mbim_rx,
+	.tx_fixup = mbim_tx_fixup,
+};