Merge "mhi: dev: netdev: add support for recycle buffers"

// SPDX-License-Identifier: GPL-2.0-only

/* Copyright (c) 2018, The Linux Foundation. All rights reserved.*/

/* Copyright (c) 2018-2019, The Linux Foundation. All rights reserved.*/

#include <linux/module.h>

#include <linux/kernel.h>

#define MHI_NETDEV_DRIVER_NAME "mhi_netdev"

#define WATCHDOG_TIMEOUT (30 * HZ)

#define IPC_LOG_PAGES (100)

#define MAX_NETBUF_SIZE (128)

#ifdef CONFIG_MHI_DEBUG

	int wake;

	u32 mru;

	u32 order;

	const char *interface_name;

	struct napi_struct napi;

	struct net_device *ndev;

	struct mhi_netbuf **netbuf_pool;

	int pool_size; /* must be power of 2 */

	int current_index;

	struct dentry *dentry;

	enum MHI_DEBUG_LEVEL msg_lvl;

	enum MHI_DEBUG_LEVEL ipc_log_lvl;

	struct mhi_netdev *mhi_netdev;

};

/* Try not to make this structure bigger than 128 bytes, since this take space

 * in payload packet.

 * Example: If MRU = 16K, effective MRU = 16K - sizeof(mhi_netbuf)

 */

struct mhi_netbuf {

	struct mhi_buf mhi_buf; /* this must be first element */

	void (*unmap)(struct device *dev, dma_addr_t addr, size_t size,

		      enum dma_data_direction dir);

};

static struct mhi_driver mhi_netdev_driver;

static void mhi_netdev_create_debugfs(struct mhi_netdev *mhi_netdev);

static __be16 mhi_netdev_ip_type_trans(struct sk_buff *skb)

static __be16 mhi_netdev_ip_type_trans(u8 data)

{

	__be16 protocol = 0;

	/* determine L3 protocol */

	switch (skb->data[0] & 0xf0) {

	switch (data & 0xf0) {

	case 0x40:

		protocol = htons(ETH_P_IP);

		break;

	return protocol;

}

static int mhi_netdev_alloc_skb(struct mhi_netdev *mhi_netdev, gfp_t gfp_t)

static struct mhi_netbuf *mhi_netdev_alloc(struct device *dev,

					   gfp_t gfp,

					   unsigned int order)

{

	struct page *page;

	struct mhi_netbuf *netbuf;

	struct mhi_buf *mhi_buf;

	void *vaddr;

	page = __dev_alloc_pages(gfp, order);

	if (!page)

		return NULL;

	vaddr = page_address(page);

	/* we going to use the end of page to store cached data */

	netbuf = vaddr + (PAGE_SIZE << order) - sizeof(*netbuf);

	mhi_buf = (struct mhi_buf *)netbuf;

	mhi_buf->page = page;

	mhi_buf->buf = vaddr;

	mhi_buf->len = (void *)netbuf - vaddr;

	mhi_buf->dma_addr = dma_map_page(dev, page, 0, mhi_buf->len,

					 DMA_FROM_DEVICE);

	if (dma_mapping_error(dev, mhi_buf->dma_addr)) {

		__free_pages(mhi_buf->page, order);

		return NULL;

	}

	return netbuf;

}

static void mhi_netdev_unmap_page(struct device *dev,

				  dma_addr_t dma_addr,

				  size_t len,

				  enum dma_data_direction dir)

{

	dma_unmap_page(dev, dma_addr, len, dir);

}

static int mhi_netdev_tmp_alloc(struct mhi_netdev *mhi_netdev, int nr_tre)

{

	u32 cur_mru = mhi_netdev->mru;

	struct mhi_device *mhi_dev = mhi_netdev->mhi_dev;

	int ret;

	struct sk_buff *skb;

	int no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE);

	struct device *dev = mhi_dev->dev.parent;

	const u32 order = mhi_netdev->order;

	int i, ret;

	for (i = 0; i < nr_tre; i++) {

		struct mhi_buf *mhi_buf;

		struct mhi_netbuf *netbuf = mhi_netdev_alloc(dev, GFP_ATOMIC,

							     order);

		if (!netbuf)

			return -ENOMEM;

		mhi_buf = (struct mhi_buf *)netbuf;

		netbuf->unmap = mhi_netdev_unmap_page;

		ret = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, mhi_buf,

					 mhi_buf->len, MHI_EOT);

		if (unlikely(ret)) {

			MSG_ERR("Failed to queue transfer, ret:%d\n", ret);

			mhi_netdev_unmap_page(dev, mhi_buf->dma_addr,

					      mhi_buf->len, DMA_FROM_DEVICE);

			__free_pages(mhi_buf->page, order);

			return ret;

		}

	}

	return 0;

}

static void mhi_netdev_queue(struct mhi_netdev *mhi_netdev)

{

	struct mhi_device *mhi_dev = mhi_netdev->mhi_dev;

	struct device *dev = mhi_dev->dev.parent;

	struct mhi_netbuf *netbuf;

	struct mhi_buf *mhi_buf;

	struct mhi_netbuf **netbuf_pool = mhi_netdev->netbuf_pool;

	int nr_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE);

	int i, peak, cur_index, ret;

	const int pool_size = mhi_netdev->pool_size - 1, max_peak = 4;

	MSG_VERB("Enter free_desc:%d\n", nr_tre);

	if (!nr_tre)

		return;

	/* try going thru reclaim pool first */

	for (i = 0; i < nr_tre; i++) {

		/* peak for the next buffer, we going to peak several times,

		 * and we going to give up if buffers are not yet free

		 */

		cur_index = mhi_netdev->current_index;

		netbuf = NULL;

		for (peak = 0; peak < max_peak; peak++) {

			struct mhi_netbuf *tmp = netbuf_pool[cur_index];

			mhi_buf = &tmp->mhi_buf;

			cur_index = (cur_index + 1) & pool_size;

			/* page == 1 idle, buffer is free to reclaim */

			if (page_ref_count(mhi_buf->page) == 1) {

				netbuf = tmp;

				break;

			}

		}

		/* could not find a free buffer */

		if (!netbuf)

			break;

		/* increment reference count so when network stack is done

		 * with buffer, the buffer won't be freed

		 */

		page_ref_inc(mhi_buf->page);

		dma_sync_single_for_device(dev, mhi_buf->dma_addr, mhi_buf->len,

					   DMA_FROM_DEVICE);

		ret = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, mhi_buf,

					 mhi_buf->len, MHI_EOT);

		if (unlikely(ret)) {

			MSG_ERR("Failed to queue buffer, ret:%d\n", ret);

			netbuf->unmap(dev, mhi_buf->dma_addr, mhi_buf->len,

				      DMA_FROM_DEVICE);

			page_ref_dec(mhi_buf->page);

			return;

		}

		mhi_netdev->current_index = cur_index;

	}

	/* recyling did not work, buffers are still busy allocate temp pkts */

	if (i < nr_tre)

		mhi_netdev_tmp_alloc(mhi_netdev, nr_tre - i);

}

/* allocating pool of memory */

static int mhi_netdev_alloc_pool(struct mhi_netdev *mhi_netdev)

{

	int i;

	struct mhi_netbuf *netbuf, **netbuf_pool;

	struct mhi_buf *mhi_buf;

	const u32 order = mhi_netdev->order;

	struct device *dev = mhi_netdev->mhi_dev->dev.parent;

	for (i = 0; i < no_tre; i++) {

		skb = alloc_skb(cur_mru, gfp_t);

		if (!skb)

	netbuf_pool = kmalloc_array(mhi_netdev->pool_size, sizeof(*netbuf_pool),

				    GFP_KERNEL);

	if (!netbuf_pool)

		return -ENOMEM;

		skb->dev = mhi_netdev->ndev;

	for (i = 0; i < mhi_netdev->pool_size; i++) {

		/* allocate paged data */

		netbuf = mhi_netdev_alloc(dev, GFP_KERNEL, order);

		if (!netbuf)

			goto error_alloc_page;

		ret = mhi_queue_transfer(mhi_dev, DMA_FROM_DEVICE, skb, cur_mru,

					 MHI_EOT);

		netbuf->unmap = dma_sync_single_for_cpu;

		netbuf_pool[i] = netbuf;

	}

		if (ret) {

			MSG_ERR("Failed to queue skb, ret:%d\n", ret);

			ret = -EIO;

			goto error_queue;

	mhi_netdev->netbuf_pool = netbuf_pool;

	return 0;

error_alloc_page:

	for (--i; i >= 0; i--) {

		netbuf = netbuf_pool[i];

		mhi_buf = &netbuf->mhi_buf;

		dma_unmap_page(dev, mhi_buf->dma_addr, mhi_buf->len,

			       DMA_FROM_DEVICE);

		__free_pages(mhi_buf->page, order);

	}

	kfree(netbuf_pool);

	return -ENOMEM;

}

	return 0;

static void mhi_netdev_free_pool(struct mhi_netdev *mhi_netdev)

{

	int i;

	struct mhi_netbuf *netbuf, **netbuf_pool = mhi_netdev->netbuf_pool;

	struct device *dev = mhi_netdev->mhi_dev->dev.parent;

	struct mhi_buf *mhi_buf;

error_queue:

	dev_kfree_skb_any(skb);

	for (i = 0; i < mhi_netdev->pool_size; i++) {

		netbuf = netbuf_pool[i];

		mhi_buf = &netbuf->mhi_buf;

		dma_unmap_page(dev, mhi_buf->dma_addr, mhi_buf->len,

			       DMA_FROM_DEVICE);

		__free_pages(mhi_buf->page, mhi_netdev->order);

	}

	return ret;

	kfree(mhi_netdev->netbuf_pool);

	mhi_netdev->netbuf_pool = NULL;

}

static int mhi_netdev_poll(struct napi_struct *napi, int budget)

	}

	/* queue new buffers */

	mhi_netdev_alloc_skb(mhi_netdev, GFP_ATOMIC);

	mhi_netdev_queue(mhi_netdev);

	/* complete work if # of packet processed less than allocated budget */

	if (rx_work < budget)

	char ifalias[IFALIASZ];

	char ifname[IFNAMSIZ];

	struct mhi_device *mhi_dev = mhi_netdev->mhi_dev;

	int no_tre;

	MSG_LOG("Prepare the channels for transfer\n");

	struct device_node *of_node = mhi_dev->dev.of_node;

	struct mhi_netdev_priv *mhi_netdev_priv;

	ret = mhi_prepare_for_transfer(mhi_dev);

	if (ret) {

		MSG_ERR("Failed to start TX chan ret %d\n", ret);

		goto mhi_failed_to_start;

	}

	mhi_netdev->alias = of_alias_get_id(of_node, "mhi_netdev");

	if (mhi_netdev->alias < 0)

		return -ENODEV;

	/* first time enabling the node */

	if (!mhi_netdev->ndev) {

		struct mhi_netdev_priv *mhi_netdev_priv;

	ret = of_property_read_string(of_node, "mhi,interface-name",

				      &mhi_netdev->interface_name);

	if (ret)

		mhi_netdev->interface_name = mhi_netdev_driver.driver.name;

	snprintf(ifalias, sizeof(ifalias), "%s_%04x_%02u.%02u.%02u_%u",

			 mhi_netdev->interface_name, mhi_dev->dev_id,

			 mhi_dev->domain, mhi_dev->bus, mhi_dev->slot,

			 mhi_netdev->alias);

		 mhi_netdev->interface_name, mhi_dev->dev_id, mhi_dev->domain,

		 mhi_dev->bus, mhi_dev->slot, mhi_netdev->alias);

		snprintf(ifname, sizeof(ifname), "%s%%d",

			 mhi_netdev->interface_name);

	snprintf(ifname, sizeof(ifname), "%s%%d", mhi_netdev->interface_name);

	rtnl_lock();

	mhi_netdev->ndev = alloc_netdev(sizeof(*mhi_netdev_priv),

					ifname, NET_NAME_PREDICTABLE,

					mhi_netdev_setup);

	if (!mhi_netdev->ndev) {

			ret = -ENOMEM;

		rtnl_unlock();

			goto net_dev_alloc_fail;

		return -ENOMEM;

	}

	mhi_netdev->ndev->mtu = mhi_dev->mtu;

		MSG_ERR("Network device registration failed\n");

		goto net_dev_reg_fail;

	}

	}

	/* queue buffer for rx path */

	no_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE);

	mhi_netdev_alloc_skb(mhi_netdev, GFP_KERNEL);

	napi_enable(&mhi_netdev->napi);

	free_netdev(mhi_netdev->ndev);

	mhi_netdev->ndev = NULL;

net_dev_alloc_fail:

	mhi_unprepare_from_transfer(mhi_dev);

mhi_failed_to_start:

	MSG_ERR("Exited ret %d.\n", ret);

	return ret;

}

		netif_wake_queue(ndev);

}

static void mhi_netdev_push_skb(struct mhi_netdev *mhi_netdev,

				struct mhi_buf *mhi_buf,

				struct mhi_result *mhi_result)

{

	struct sk_buff *skb;

	skb = alloc_skb(0, GFP_ATOMIC);

	if (!skb) {

		__free_pages(mhi_buf->page, mhi_netdev->order);

		return;

	}

	skb_add_rx_frag(skb, 0, mhi_buf->page, 0,

			mhi_result->bytes_xferd, mhi_netdev->mru);

	skb->dev = mhi_netdev->ndev;

	skb->protocol = mhi_netdev_ip_type_trans(*(u8 *)mhi_buf->buf);

	netif_receive_skb(skb);

}

static void mhi_netdev_xfer_dl_cb(struct mhi_device *mhi_dev,

				  struct mhi_result *mhi_result)

{

	struct mhi_netdev *mhi_netdev = mhi_device_get_devdata(mhi_dev);

	struct sk_buff *skb = mhi_result->buf_addr;

	struct net_device *dev = mhi_netdev->ndev;

	struct mhi_netbuf *netbuf = mhi_result->buf_addr;

	struct mhi_buf *mhi_buf = &netbuf->mhi_buf;

	struct net_device *ndev = mhi_netdev->ndev;

	struct device *dev = mhi_dev->dev.parent;

	netbuf->unmap(dev, mhi_buf->dma_addr, mhi_buf->len, DMA_FROM_DEVICE);

	/* modem is down, drop the buffer */

	if (mhi_result->transaction_status == -ENOTCONN) {

		dev_kfree_skb(skb);

		__free_pages(mhi_buf->page, mhi_netdev->order);

		return;

	}

	skb_put(skb, mhi_result->bytes_xferd);

	dev->stats.rx_packets++;

	dev->stats.rx_bytes += mhi_result->bytes_xferd;

	skb->protocol = mhi_netdev_ip_type_trans(skb);

	netif_receive_skb(skb);

	ndev->stats.rx_packets++;

	ndev->stats.rx_bytes += mhi_result->bytes_xferd;

	mhi_netdev_push_skb(mhi_netdev, mhi_buf, mhi_result);

}

static void mhi_netdev_status_cb(struct mhi_device *mhi_dev, enum MHI_CB mhi_cb)

	unregister_netdev(mhi_netdev->ndev);

	netif_napi_del(&mhi_netdev->napi);

	free_netdev(mhi_netdev->ndev);

	mhi_netdev_free_pool(mhi_netdev);

	if (!IS_ERR_OR_NULL(mhi_netdev->dentry))

		debugfs_remove_recursive(mhi_netdev->dentry);

	int ret;

	struct mhi_netdev *mhi_netdev;

	struct device_node *of_node = mhi_dev->dev.of_node;

	int nr_tre;

	char node_name[32];

	if (!of_node)

	if (!mhi_netdev)

		return -ENOMEM;

	mhi_netdev->alias = of_alias_get_id(of_node, "mhi_netdev");

	if (mhi_netdev->alias < 0)

		return -ENODEV;

	mhi_netdev->mhi_dev = mhi_dev;

	mhi_device_set_devdata(mhi_dev, mhi_netdev);

	if (ret)

		return -ENODEV;

	ret = of_property_read_string(of_node, "mhi,interface-name",

				      &mhi_netdev->interface_name);

	/* MRU must be multiplication of page size */

	mhi_netdev->order = __ilog2_u32(mhi_netdev->mru / PAGE_SIZE);

	if ((PAGE_SIZE << mhi_netdev->order) < mhi_netdev->mru)

		return -EINVAL;

	ret = mhi_netdev_enable_iface(mhi_netdev);

	if (ret)

		mhi_netdev->interface_name = mhi_netdev_driver.driver.name;

		return ret;

	/* create ipc log buffer */

	snprintf(node_name, sizeof(node_name), "%s_%04x_%02u.%02u.%02u_%u",

		 mhi_netdev->interface_name, mhi_dev->dev_id, mhi_dev->domain,

		 mhi_dev->bus, mhi_dev->slot, mhi_netdev->alias);

	mhi_netdev->ipc_log = ipc_log_context_create(IPC_LOG_PAGES, node_name,

						     0);

	mhi_netdev->ipc_log = ipc_log_context_create(IPC_LOG_PAGES,

						     node_name, 0);

	mhi_netdev->msg_lvl = MHI_MSG_LVL_ERROR;

	mhi_netdev->ipc_log_lvl = IPC_LOG_LVL;

	/* setup network interface */

	ret = mhi_netdev_enable_iface(mhi_netdev);

	mhi_netdev_create_debugfs(mhi_netdev);

	/* move mhi channels to start state */

	ret = mhi_prepare_for_transfer(mhi_dev);

	if (ret) {

		MSG_ERR("Failed to start channels ret %d\n", ret);

		goto error_start;

	}

	/* setup pool size ~2x ring length*/

	nr_tre = mhi_get_no_free_descriptors(mhi_dev, DMA_FROM_DEVICE);

	mhi_netdev->pool_size = 1 << __ilog2_u32(nr_tre);

	if (nr_tre > mhi_netdev->pool_size)

		mhi_netdev->pool_size <<= 1;

	mhi_netdev->pool_size <<= 1;

	/* allocate memory pool */

	ret = mhi_netdev_alloc_pool(mhi_netdev);

	if (ret)

		return ret;

		goto error_start;

	mhi_netdev_create_debugfs(mhi_netdev);

	/* now we have a pool of buffers allocated, queue to hardware

	 * by triggering a napi_poll

	 */

	napi_schedule(&mhi_netdev->napi);

	return 0;

error_start:

	netif_stop_queue(mhi_netdev->ndev);

	napi_disable(&mhi_netdev->napi);

	unregister_netdev(mhi_netdev->ndev);

	netif_napi_del(&mhi_netdev->napi);

	free_netdev(mhi_netdev->ndev);

	return ret;

}

static const struct mhi_device_id mhi_netdev_match_table[] = {

static int __init mhi_netdev_init(void)

{

	BUILD_BUG_ON(sizeof(struct mhi_netbuf) > MAX_NETBUF_SIZE);

	mhi_netdev_create_debugfs_dir();

	return mhi_driver_register(&mhi_netdev_driver);