Commit d816f2a9 authored by David S. Miller's avatar David S. Miller
Browse files

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



Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parents 24a1720a 163c5e62

drivers/net/Makefile

+1 −1
Original line number Diff line number Diff line
@@ -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

0 → 100644
+3 −0
Original line number Diff line number Diff line 
obj-$(CONFIG_MHI_NET) += mhi_net.o



mhi_net-y := net.o proto_mbim.o

drivers/net/mhi/mhi.h

0 → 100644
+40 −0
Original line number Diff line number Diff line 
/* 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

+57 −33
Original line number Diff line number Diff line
@@ -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,6 +215,9 @@ static void mhi_net_dl_callback(struct mhi_device *mhi_dev, 
			break;

		}



		if (proto && proto->rx)

			proto->rx(mhi_netdev, skb);

		else

			netif_rx(skb);

	}
@@ -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

0 → 100644
+293 −0
Original line number Diff line number Diff line 
// 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,

};