Merge "mhi: dev: netdev: add debug stats to monitor buffers allocated"

#include <linux/errno.h>

#include <linux/of_device.h>

#include <linux/rtnetlink.h>

#include <linux/kthread.h>

#include <linux/mhi.h>

#define MHI_NETDEV_DRIVER_NAME "mhi_netdev"

	int alias;

	struct mhi_device *mhi_dev;

	struct mhi_netdev *rsc_dev; /* rsc linked node */

	struct mhi_netdev *rsc_parent;

	bool is_rsc_dev;

	int wake;

	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 list_head *recycle_pool;

	int pool_size;

	bool chain_skb;

	struct mhi_net_chain *chain;

	struct task_struct *alloc_task;

	wait_queue_head_t alloc_event;

	int bg_pool_limit; /* minimum pool size */

	int bg_pool_size; /* current size of the pool */

	struct list_head *bg_pool;

	spinlock_t bg_lock; /* lock to access list */

	struct dentry *dentry;

	enum MHI_DEBUG_LEVEL msg_lvl;

	enum MHI_DEBUG_LEVEL ipc_log_lvl;

	void *ipc_log;

	/* debug stats */

	u32 abuffers, kbuffers, rbuffers;

};

struct mhi_netdev_priv {

 */

struct mhi_netbuf {

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

	bool recycle;

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

		      enum dma_data_direction dir);

};

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

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

	netbuf->recycle = false;

	mhi_buf = (struct mhi_buf *)netbuf;

	mhi_buf->page = page;

	mhi_buf->buf = vaddr;

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

	if (!dev)

		return netbuf;

	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)) {

	dma_unmap_page(dev, dma_addr, len, dir);

}

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

static int mhi_netdev_tmp_alloc(struct mhi_netdev *mhi_netdev,

				struct mhi_device *mhi_dev,

				int nr_tre)

{

	struct mhi_device *mhi_dev = mhi_netdev->mhi_dev;

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

	const u32 order = mhi_netdev->order;

	int i, ret;

			__free_pages(mhi_buf->page, order);

			return ret;

		}

		mhi_netdev->abuffers++;

	}

	return 0;

}

static void mhi_netdev_queue(struct mhi_netdev *mhi_netdev)

static int mhi_netdev_queue_bg_pool(struct mhi_netdev *mhi_netdev,

				    struct mhi_device *mhi_dev,

				    int nr_tre)

{

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

	int i, ret;

	LIST_HEAD(head);

	spin_lock_bh(&mhi_netdev->bg_lock);

	list_splice_init(mhi_netdev->bg_pool, &head);

	spin_unlock_bh(&mhi_netdev->bg_lock);

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

		struct mhi_buf *mhi_buf =

			list_first_entry_or_null(&head, struct mhi_buf, node);

		struct mhi_netbuf *netbuf = (struct mhi_netbuf *)mhi_buf;

		if (!mhi_buf)

			break;

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

						 mhi_buf->len, DMA_FROM_DEVICE);

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

			break;

		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);

			break;

		}

		list_del(&mhi_buf->node);

		mhi_netdev->kbuffers++;

	}

	/* add remaining buffers back to main pool */

	spin_lock_bh(&mhi_netdev->bg_lock);

	list_splice(&head, mhi_netdev->bg_pool);

	mhi_netdev->bg_pool_size -= i;

	spin_unlock_bh(&mhi_netdev->bg_lock);

	/* wake up the bg thread to allocate more buffers */

	wake_up_interruptible(&mhi_netdev->alloc_event);

	return i;

}

static void mhi_netdev_queue(struct mhi_netdev *mhi_netdev,

			     struct mhi_device *mhi_dev)

{

	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;

	struct list_head *pool = mhi_netdev->recycle_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;

	int i, ret;

	const int  max_peek = 4;

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

	/* try going thru reclaim pool first */

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

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

		/* peek 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;

		int peek = 0;

		netbuf = NULL;

		list_for_each_entry(mhi_buf, pool, node) {

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

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

				netbuf = tmp;

				netbuf = (struct mhi_netbuf *)mhi_buf;

				break;

			}

			if (peek++ >= max_peek)

				break;

		}

		/* could not find a free buffer */

		 * with buffer, the buffer won't be freed

		 */

		page_ref_inc(mhi_buf->page);

		list_del(&mhi_buf->node);

		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,

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

				      DMA_FROM_DEVICE);

			page_ref_dec(mhi_buf->page);

			list_add(&mhi_buf->node, pool);

			return;

		}

		mhi_netdev->current_index = cur_index;

		mhi_netdev->rbuffers++;

	}

	/* recycling did not work, buffers are still busy use bg pool */

	if (i < nr_tre)

		i += mhi_netdev_queue_bg_pool(mhi_netdev, mhi_dev, nr_tre - i);

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

	if (i < nr_tre)

		mhi_netdev_tmp_alloc(mhi_netdev, nr_tre - i);

		mhi_netdev_tmp_alloc(mhi_netdev, mhi_dev, 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;

	struct mhi_netbuf *netbuf;

	struct mhi_buf *mhi_buf, *tmp;

	const u32 order = mhi_netdev->order;

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

	struct list_head *pool = kmalloc(sizeof(*pool), GFP_KERNEL);

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

				    GFP_KERNEL);

	if (!netbuf_pool)

	if (!pool)

		return -ENOMEM;

	INIT_LIST_HEAD(pool);

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

		/* allocate paged data */

		netbuf = mhi_netdev_alloc(dev, GFP_KERNEL, order);

			goto error_alloc_page;

		netbuf->unmap = dma_sync_single_for_cpu;

		netbuf_pool[i] = netbuf;

		netbuf->recycle = true;

		mhi_buf = (struct mhi_buf *)netbuf;

		list_add(&mhi_buf->node, pool);

	}

	mhi_netdev->netbuf_pool = netbuf_pool;

	mhi_netdev->recycle_pool = pool;

	return 0;

error_alloc_page:

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

		netbuf = netbuf_pool[i];

		mhi_buf = &netbuf->mhi_buf;

	list_for_each_entry_safe(mhi_buf, tmp, pool, node) {

		list_del(&mhi_buf->node);

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

			       DMA_FROM_DEVICE);

		__free_pages(mhi_buf->page, order);

	}

	kfree(netbuf_pool);

	kfree(pool);

	return -ENOMEM;

}

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;

	struct mhi_buf *mhi_buf, *tmp;

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

		netbuf = netbuf_pool[i];

		mhi_buf = &netbuf->mhi_buf;

	list_for_each_entry_safe(mhi_buf, tmp, mhi_netdev->recycle_pool, node) {

		list_del(&mhi_buf->node);

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

			       DMA_FROM_DEVICE);

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

	}

	kfree(mhi_netdev->netbuf_pool);

	mhi_netdev->netbuf_pool = NULL;

	kfree(mhi_netdev->recycle_pool);

	/* free the bg pool */

	list_for_each_entry_safe(mhi_buf, tmp, mhi_netdev->bg_pool, node) {

		list_del(&mhi_buf->node);

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

		mhi_netdev->bg_pool_size--;

	}

}

static int mhi_netdev_alloc_thread(void *data)

{

	struct mhi_netdev *mhi_netdev = data;

	struct mhi_netbuf *netbuf;

	struct mhi_buf *mhi_buf, *tmp_buf;

	const u32 order = mhi_netdev->order;

	LIST_HEAD(head);

	while (!kthread_should_stop()) {

		while (mhi_netdev->bg_pool_size <= mhi_netdev->bg_pool_limit) {

			int buffers = 0, i;

			/* do a bulk allocation */

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

				if (kthread_should_stop())

					goto exit_alloc;

				netbuf = mhi_netdev_alloc(NULL, GFP_KERNEL,

							  order);

				if (!netbuf)

					continue;

				mhi_buf = (struct mhi_buf *)netbuf;

				list_add(&mhi_buf->node, &head);

				buffers++;

			}

			/* add the list to main pool */

			spin_lock_bh(&mhi_netdev->bg_lock);

			list_splice_init(&head, mhi_netdev->bg_pool);

			mhi_netdev->bg_pool_size += buffers;

			spin_unlock_bh(&mhi_netdev->bg_lock);

		}

		/* replenish the ring */

		napi_schedule(mhi_netdev->napi);

		/* wait for buffers to run low or thread to stop */

		wait_event_interruptible(mhi_netdev->alloc_event,

			kthread_should_stop() ||

			mhi_netdev->bg_pool_size <= mhi_netdev->bg_pool_limit);

	}

exit_alloc:

	list_for_each_entry_safe(mhi_buf, tmp_buf, &head, node) {

		list_del(&mhi_buf->node);

		__free_pages(mhi_buf->page, order);

	}

	return 0;

}

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

	}

	/* queue new buffers */

	mhi_netdev_queue(mhi_netdev);

	mhi_netdev_queue(mhi_netdev, mhi_dev);

	if (rsc_dev)

		mhi_netdev_queue(rsc_dev);

		mhi_netdev_queue(mhi_netdev, rsc_dev->mhi_dev);

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

	if (rx_work < budget)

	struct mhi_net_chain *chain = mhi_netdev->chain;

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

	if (likely(netbuf->recycle))

		list_add_tail(&mhi_buf->node, mhi_netdev->recycle_pool);

	/* modem is down, drop the buffer */

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

struct dentry *dentry;

static int mhi_netdev_debugfs_stats_show(struct seq_file *m, void *d)

{

	struct mhi_netdev *mhi_netdev = m->private;

	seq_printf(m,

		   "mru:%u order:%u pool_size:%d, bg_pool_size:%d bg_pool_limit:%d abuf:%u kbuf:%u rbuf:%u\n",

		   mhi_netdev->mru, mhi_netdev->order, mhi_netdev->pool_size,

		   mhi_netdev->bg_pool_size, mhi_netdev->bg_pool_limit,

		   mhi_netdev->abuffers, mhi_netdev->kbuffers,

		   mhi_netdev->rbuffers);

	return 0;

}

static int mhi_netdev_debugfs_stats_open(struct inode *inode, struct file *fp)

{

	return single_open(fp, mhi_netdev_debugfs_stats_show, inode->i_private);

}

static const struct file_operations debugfs_stats = {

	.open = mhi_netdev_debugfs_stats_open,

	.release = single_release,

	.read = seq_read,

};

static void mhi_netdev_create_debugfs(struct mhi_netdev *mhi_netdev)

{

	char node_name[32];

	mhi_netdev->dentry = debugfs_create_dir(node_name, dentry);

	if (IS_ERR_OR_NULL(mhi_netdev->dentry))

		return;

	debugfs_create_file_unsafe("stats", 0444, mhi_netdev->dentry,

				   mhi_netdev, &debugfs_stats);

}

static void mhi_netdev_create_debugfs_dir(void)

		return;

	}

	kthread_stop(mhi_netdev->alloc_task);

	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);

	mhi_netdev_free_pool(mhi_netdev);

	if (!IS_ERR_OR_NULL(mhi_netdev->dentry))

		debugfs_remove_recursive(mhi_netdev->dentry);

	mhi_netdev->ipc_log_lvl = parent->ipc_log_lvl;

	mhi_netdev->is_rsc_dev = true;

	mhi_netdev->chain = parent->chain;

	mhi_netdev->rsc_parent = parent;

	mhi_netdev->recycle_pool = parent->recycle_pool;

	mhi_netdev->bg_pool = parent->bg_pool;

}

static int mhi_netdev_probe(struct mhi_device *mhi_dev,

	if (!mhi_netdev)

		return -ENOMEM;

	/* 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);

		return ret;

	}

	mhi_netdev->mhi_dev = mhi_dev;

	mhi_device_set_devdata(mhi_dev, mhi_netdev);

		if (ret)

			return ret;

		/* 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;

		/* if we expect child device to share then double the pool */

		if (of_parse_phandle(of_node, "mhi,rsc-child", 0))

			mhi_netdev->pool_size <<= 1;

		/* allocate memory pool */

		ret = mhi_netdev_alloc_pool(mhi_netdev);

		if (ret)

			return -ENOMEM;

		/* create a background task to allocate memory */

		mhi_netdev->bg_pool = kmalloc(sizeof(*mhi_netdev->bg_pool),

					      GFP_KERNEL);

		if (!mhi_netdev->bg_pool)

			return -ENOMEM;

		init_waitqueue_head(&mhi_netdev->alloc_event);

		INIT_LIST_HEAD(mhi_netdev->bg_pool);

		spin_lock_init(&mhi_netdev->bg_lock);

		mhi_netdev->bg_pool_limit = mhi_netdev->pool_size / 4;

		mhi_netdev->alloc_task = kthread_run(mhi_netdev_alloc_thread,

						     mhi_netdev,

						     mhi_netdev->ndev->name);

		if (IS_ERR(mhi_netdev->alloc_task))

			return PTR_ERR(mhi_netdev->alloc_task);

		/* create ipc log buffer */

		snprintf(node_name, sizeof(node_name),

			 "%s_%04x_%02u.%02u.%02u_%u",

		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)

		goto error_start;

	/* link child node with parent node if it's children dev */

	if (p_netdev)

		p_netdev->rsc_dev = mhi_netdev;

	napi_schedule(mhi_netdev->napi);

	return 0;

error_start:

	if (phandle)

		return ret;

	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[] = {