package/kernel/mfp/files/fp_device.c

/*
 *	Fastpath Devices
 *
 *	This program is free software; you can redistribute it and/or
 *	modify it under the terms of the GNU FP_ERR( Public License
 *	as published by the Free Software Foundation; either version
 *	2 of the License, or (at your option) any later version.
 */

#define pr_fmt(fmt) "mfp" " device:%s:%d: " fmt, __func__, __LINE__

#include <net/ipv6.h>
#include <linux/inet.h>
#include "fp_common.h"
#include "fp_device.h"
#include "fp_core.h"
#include "fp_ndisc.h"

static struct fastpath_module *fp_device;

#define STATS_TITLE_FMT \
	"%-13.13s:              total              Slow                Fast\n"
#define STATS_DATA_FMT \
	"   %-10.10s:       %10llu       %10llu          %10lu\n"

static unsigned long long inline stats_diff(unsigned long long slow, unsigned long fast)
{
	return (slow < fast) ? 0 : slow - fast;
}

static int
add_stats_to_buff(char *buf, struct fp_net_device *fdev, ssize_t max_size)
{
	struct rtnl_link_stats64 temp;
	const struct rtnl_link_stats64 *stats;
	struct fp_net_device_stats *stats_fast;
	static const char *title_fmt = STATS_TITLE_FMT;
	static const char *data_fmt = STATS_DATA_FMT;
	int len;

	stats = dev_get_stats(fdev->dev, &temp);

	stats_fast = &fdev->stats;

	len = scnprintf(buf, max_size, title_fmt, fdev->dev->name);

	len += scnprintf(buf + len, max_size - len, data_fmt, "queue_stopped",
			   0llu, 0llu, stats_fast->queue_stopped);
	len += scnprintf(buf + len, max_size - len, data_fmt, "rx_packets",
			   stats->rx_packets, stats_diff(stats->rx_packets, stats_fast->rx_packets) ,stats_fast->rx_packets);
	len += scnprintf(buf + len, max_size - len, data_fmt, "rx_bytes",
			   stats->rx_bytes, stats_diff(stats->rx_bytes, stats_fast->rx_bytes), stats_fast->rx_bytes);
	len += scnprintf(buf + len, max_size - len, data_fmt, "rx_errors",
			   stats->rx_errors, stats_diff(stats->rx_errors, stats_fast->rx_errors), stats_fast->rx_errors);
	len += scnprintf(buf + len, max_size - len, data_fmt, "rx_dropped",
			   stats->rx_dropped, stats_diff(stats->rx_dropped, stats_fast->rx_dropped), stats_fast->rx_dropped);
	len += scnprintf(buf + len, max_size - len, data_fmt, "tx_packets",
			   stats->tx_packets, stats_diff(stats->tx_packets, stats_fast->tx_packets), stats_fast->tx_packets);
	len += scnprintf(buf + len, max_size - len, data_fmt, "tx_bytes",
			   stats->tx_bytes, stats_diff(stats->tx_bytes, stats_fast->tx_bytes), stats_fast->tx_bytes);
	len += scnprintf(buf + len, max_size - len, data_fmt, "tx_errors",
			   stats->tx_errors, stats_diff(stats->tx_errors, stats_fast->tx_errors), stats_fast->tx_errors);
	len += scnprintf(buf + len, max_size - len, data_fmt, "tx_dropped",
			   stats->tx_dropped, stats_diff(stats->tx_dropped, stats_fast->tx_dropped), stats_fast->tx_dropped);
	return len;
}

static int
add_status_to_buff(char *buf, struct fp_net_device *fdev, ssize_t max_size)
{
	return scnprintf(buf, max_size, "%16s%8s%11s%9d%9d%9s\n",
			fdev->dev->name,
			netif_running(fdev->dev) ? "Up" : "Down",
			fdev->forward ? "enabled" : "disabled",
			atomic_read(&fdev->refcnt),
			netdev_refcnt_read(fdev->dev),
			fdev->br ? fdev->br->name : "NA");
}

static inline bool ip6addr_is_empty(struct in6_addr *addr)
{
	return !addr->in6_u.u6_addr32[0] &&
	       !addr->in6_u.u6_addr32[1] &&
	       !addr->in6_u.u6_addr32[2] &&
	       !addr->in6_u.u6_addr32[3];
}

static ssize_t fdev_forward_show(struct fp_net_device *fdev, char *buf)
{
	return scnprintf(buf, PAGE_SIZE, "%s\n", fdev->forward ? "Enabled" : "Disabled");
}

static ssize_t fdev_forward_store(struct fp_net_device *fdev,
				   const char *buf, size_t count)
{
	unsigned int forward;

	if (sscanf(buf, "%u", &forward) != 1)
		return -EINVAL;

	fdev->forward = (bool)forward;

	return count;
}

/**
 * show statistics
 */
static ssize_t fdev_stats_show(struct fp_net_device *fdev, char *buf)
{
	struct net_device_stats *stats_slow = &fdev->dev->stats;
	struct fp_net_device_stats *stats_fast = &fdev->stats;

	if (fdev->dev->netdev_ops && fdev->dev->netdev_ops->ndo_get_stats)
		stats_slow = fdev->dev->netdev_ops->ndo_get_stats(fdev->dev);
	stats_fast = &fdev->stats;

	return add_stats_to_buff(buf, fdev, PAGE_SIZE - 1);;
}

/**
 * clear statistics
 * 0 - clear fast stats only
 * 1 - clear slow & fast stats
 */
static ssize_t fdev_stats_store(struct fp_net_device *fdev,
				   const char *buf, size_t count)
{
	struct net_device_stats *stats_slow = &fdev->dev->stats;
	struct fp_net_device_stats *stats_fast = &fdev->stats;
	unsigned int op;

	if (sscanf(buf, "%u", &op) != 1 || op > 1)
		return -EINVAL;

	if (fdev->dev->netdev_ops && fdev->dev->netdev_ops->ndo_get_stats)
		stats_slow = fdev->dev->netdev_ops->ndo_get_stats(fdev->dev);
	stats_fast = &fdev->stats;

	memset(stats_fast,0,sizeof(struct fp_net_device_stats));
	if (op)
		memset(stats_slow,0,sizeof(struct net_device_stats));

	return count;
}

/**
 * show status
 */
static ssize_t fdev_status_show(struct fp_net_device *fdev, char *buf)
{
	int len;

	len = scnprintf(buf, PAGE_SIZE, "          device   state    forward   refcnt   dev_ref   bridge\n");
	return len + add_status_to_buff(buf + len, fdev, PAGE_SIZE - len -1);
}

static ssize_t fpdev_prefixlen_store(struct fp_net_device *fpdev,
					const char *buf, size_t count)
{
	int pref;
	sscanf(buf, "%d\n", &pref);

	fpdev->prefixlen = pref;

	return count;
}

static ssize_t fpdev_prefixlen_show(struct fp_net_device *fpdev, char *buf)
{
	return scnprintf(buf, PAGE_SIZE, "%d\n", fpdev->prefixlen);
}

static ssize_t fpdev_ll6addr_store(struct fp_net_device *fpdev,
					const char *buf, size_t count)
{
	in6_pton(buf, -1, (u8 *)&fpdev->ll6addr.s6_addr, -1, NULL);

	if (ip6addr_is_empty(&fpdev->ll6addr))
		fpdev_clear_ll6(fpdev);
	else
		fpdev_set_ll6(fpdev);

	memset(&fpdev->gb6addr, 0, sizeof(struct in6_addr));
	fpdev->prefixlen = 0;
	fpdev->mtu = 0;
	fpdev_clear_gb6(fpdev);
	fpdev_clear_mtu(fpdev);

	return count;
}

static ssize_t fpdev_ll6addr_show(struct fp_net_device *fpdev, char *buf)
{
	return scnprintf(buf, PAGE_SIZE, "%pI6c\n", &fpdev->ll6addr);
}

static ssize_t fpdev_gb6addr_store(struct fp_net_device *fpdev,
					const char *buf, size_t count)
{
	in6_pton(buf, -1, (u8 *)&fpdev->gb6addr.s6_addr, -1, NULL);

	fpdev_set_gb6(fpdev);
	return count;
}

static ssize_t fpdev_gb6addr_show(struct fp_net_device *fpdev, char *buf)
{
	return scnprintf(buf, PAGE_SIZE, "%pI6c\n", &fpdev->gb6addr);
}

static ssize_t fpdev_mtu_store(struct fp_net_device *fpdev,
					const char *buf, size_t count)
{
	u32 mtu;
	sscanf(buf, "%d\n", &mtu);

	fpdev->mtu = mtu;

	return count;
}


static ssize_t fpdev_mtu_show(struct fp_net_device *fpdev, char *buf)
{
	return scnprintf(buf, PAGE_SIZE, "%d\n", fpdev->mtu);
}

struct fp_dev_attr {
	struct attribute attr;
	ssize_t (*show)(struct fp_net_device *, char *);
	ssize_t (*store)(struct fp_net_device *, const char *, size_t count);
};

#define FPDEV_ATTR(_name, _mode, _show, _store) \
	struct fp_dev_attr fp_dev_attr_##_name = __ATTR(_name, _mode, _show, _store)

#define to_fpdev(fpdev) container_of(fpdev, struct fp_net_device, kobj)
#define to_attr(a) container_of(a, struct fp_dev_attr, attr)

static FPDEV_ATTR(forward, S_IRUGO|S_IWUSR, fdev_forward_show, fdev_forward_store);
static FPDEV_ATTR(statistics, S_IRUGO|S_IWUSR, fdev_stats_show, fdev_stats_store);
static FPDEV_ATTR(status, S_IRUGO, fdev_status_show, NULL);
static FPDEV_ATTR(ll6addr, S_IRUGO|S_IWUSR, fpdev_ll6addr_show,
		   fpdev_ll6addr_store);
static FPDEV_ATTR(gb6addr, S_IRUGO|S_IWUSR, fpdev_gb6addr_show,
		   fpdev_gb6addr_store);
static FPDEV_ATTR(prefixlen, S_IRUGO|S_IWUSR, fpdev_prefixlen_show,
		   fpdev_prefixlen_store);
static FPDEV_ATTR(mtu, S_IRUGO|S_IWUSR, fpdev_mtu_show,
		   fpdev_mtu_store);

static struct attribute *fpdev_default_attrs[] = {
	&fp_dev_attr_forward.attr,
	&fp_dev_attr_statistics.attr,
	&fp_dev_attr_status.attr,
	&fp_dev_attr_ll6addr.attr,
	&fp_dev_attr_gb6addr.attr,
	&fp_dev_attr_prefixlen.attr,
	&fp_dev_attr_mtu.attr,
	NULL
};

static ssize_t fpdev_show(struct kobject *kobj, struct attribute *attr, char *buf)
{
	struct fp_net_device *fdev = to_fpdev(kobj);
	struct fp_dev_attr *fattr = to_attr(attr);

	if (!fdev || !fattr || !fattr->show)
		return -EINVAL;

	return fattr->show(fdev, buf);
}

static ssize_t fpdev_store(struct kobject *kobj, struct attribute *attr,
			const char *buf, size_t count)
{
	struct fp_net_device *fdev = to_fpdev(kobj);
	struct fp_dev_attr *fattr = to_attr(attr);

	if (!fdev || !fattr || !fattr->store)
		return -EINVAL;
	
	return fattr->store(fdev, buf, count);
}

static const struct sysfs_ops fpdev_sysfs_ops = {
	.show = fpdev_show,
	.store = fpdev_store,
};

void destroy_fpdev(struct work_struct *w)
{
	struct fp_dev_work *work;
	struct fp_net_device *fpdev;
	struct fp_dev_list *fpdl;

	work = container_of(w, struct fp_dev_work, work.work);
	BUG_ON(!work);

	fpdev = work->fpdev;
	fpdl = work->fpdl;

	pr_err("device (%s) destroyed\n", fpdev->dev->name);

	rtnl_lock();
	dev_put(fpdev->dev);
	rtnl_unlock();

	kfree(fpdev);
	atomic_dec(&fpdl->dev_count);
	wake_up(&fpdl->wq);

	list_del(&work->list);
	kfree(work);

}

void destroy_fpdev_rcu(struct rcu_head *rcu)
{
	struct fp_dev_work *work;
	struct fp_net_device *fpdev =
		container_of(rcu, struct fp_net_device, rcu);
	struct fp_dev_list *fpdl = fp_device->priv;

	work = kzalloc(sizeof(*work), GFP_ATOMIC);
	if (!work)
		return;

	work->fpdev = fpdev;
	work->fpdl = fpdl;

	INIT_LIST_HEAD(&work->list);
	INIT_DELAYED_WORK(&work->work, destroy_fpdev);
	queue_delayed_work(fpdl->dev_put_wq, &work->work, 0);
}

static void release_fpdev(struct kobject *kobj)
{
	pr_debug("fpdev kobj released\n");
}

static struct kobj_type ktype_fpdev = {
	.sysfs_ops = &fpdev_sysfs_ops,
	.default_attrs = fpdev_default_attrs,
	.release = release_fpdev,
};

static void fpdev_del_if_finish(struct work_struct *work)
{
	struct fp_net_device *fpdev;

	fpdev = container_of(work, struct fp_net_device, free_work);

	kobject_put(&fpdev->kobj);
	fpdev_put(fpdev);
}

/*--------------------------------------------------------------*/
/*-				API				-*/
/*--------------------------------------------------------------*/

/**
 * delete the fastpath device associated with this net device
 * 
 * @param dev    net device
 * 
 * @return 0 for success, -ENODEV if not found
 */
int fpdev_del_if(struct net_device *dev)
{
	struct fp_net_device *fpdev;
	struct fp_dev_list *fpdl = fp_device->priv;

	spin_lock_bh(&fpdl->list_lock);
	rcu_read_lock_bh();
	list_for_each_entry_rcu(fpdev, &fpdl->devices_list, list) {
		if (fpdev->dev == dev && fpdev_hold(fpdev))
			goto found;
	}

	fpdev = NULL;

found:
	rcu_read_unlock_bh();

	if (!fpdev) {
		pr_debug("device (%s) not found\n", dev->name);
		spin_unlock_bh(&fpdl->list_lock);
		return -ENODEV;
	}

	list_del_rcu(&fpdev->list);
	spin_unlock_bh(&fpdl->list_lock);

	fpdev_put(fpdev);
	schedule_work(&fpdev->free_work);

	fpdev_put(fpdev);

	printk(KERN_DEBUG "device (%s) found and deleted\n", dev->name);
	return 0;
}

/**
 * create and add a fastpath device for a given interface
 * 
 * @param dev    net device
 * 
 * @return 0 for success, error code otherwise
 */
int fpdev_add_if(struct net_device *dev)
{
	struct fp_net_device *fpdev;
	struct fp_dev_list *fpdl;
	int ret;

	BUG_ON(!dev);
	BUG_ON(!fp_device);

	fpdl = fp_device->priv;

	fpdev = kzalloc(sizeof(*fpdev), GFP_ATOMIC);
	if (!fpdev) {
		ret = -ENOMEM;
		goto err;
	}

	dev_hold(dev);

	ret = kobject_init_and_add(&fpdev->kobj, &ktype_fpdev, &fp_device->kobj,
				   dev->name);
	if (ret)
		goto kobj_err;


	fpdev->forward = true;
	fpdev->dev = dev;
	INIT_LIST_HEAD(&fpdev->list);
	INIT_WORK(&fpdev->free_work, fpdev_del_if_finish);

	/* extra reference for return */
	atomic_set(&fpdev->refcnt, 2);
	atomic_inc(&fpdl->dev_count);

	spin_lock_bh(&fpdl->list_lock);
	list_add_tail_rcu(&fpdev->list, &fpdl->devices_list);
	spin_unlock_bh(&fpdl->list_lock);

	kobject_uevent(&fpdev->kobj, KOBJ_ADD);
	
	pr_debug("created fastpath device for %s\n", dev->name);

	return 0;

kobj_err:
	kobject_put(&fpdev->kobj);
	dev_put(dev);
	kfree(fpdev);
err:
	pr_err("could not creat fastpath device for %s\n", dev->name);
	return ret;
}

/**
 * search for a fastpath device associated with a given net device.
 * If found, the fastpath device's refcount is incremented.
 * The user must call fpdev_put() when finished in order to release the device.
 * 
 * @param dev    net device
 * 
 * @return pointer to the associated fastpath device (NULL if not found)
 */
struct fp_net_device *fpdev_get_if(struct net_device *dev)
{
	struct fp_net_device *fpdev;
	struct fp_dev_list *fpdl = fp_device->priv;

	rcu_read_lock_bh();
	list_for_each_entry_rcu(fpdev, &fpdl->devices_list, list) {
		if (fpdev->dev == dev && atomic_inc_not_zero(&fpdev->refcnt))
			goto found;
	}

	fpdev = NULL;
	printk(KERN_DEBUG "device (%s) not found\n", dev->name);

found:
	rcu_read_unlock_bh();
	return fpdev;
}

struct fp_net_device *fpdev_get_ccinet(void)
{
	struct fp_net_device *fpdev;
	struct fp_dev_list *fpdl = fp_device->priv;

	rcu_read_lock_bh();
	list_for_each_entry_rcu(fpdev, &fpdl->devices_list, list) {
		if (fpdev_is_gb6_set(fpdev) && fpdev_is_ll6_set(fpdev) &&
			(!strncasecmp(fpdev->dev->name, "ccinet", 6)) &&
			fpdev_is_mtu_set(fpdev) && atomic_inc_not_zero(&fpdev->refcnt))
			goto found;
	}

	fpdev = NULL;

found:
	rcu_read_unlock_bh();
	return fpdev;
}

/**
 * show statistics (all fastpath devices)
 */
static ssize_t stats_show(struct fastpath_module *m, char *buf)
{
	struct fp_net_device *itr;
	int len, res;
	struct fp_dev_list *fpdl = fp_device->priv;

	len = sprintf(buf, "fastpath statistics\n");

	spin_lock_bh(&fpdl->list_lock);
	list_for_each_entry(itr, &fpdl->devices_list, list) {
		if (!netif_running(itr->dev) || !fpdev_hold(itr))
			continue;
		res = add_stats_to_buff(buf + len, itr, PAGE_SIZE - len - 1);
		fpdev_put(itr);
		len += res;
		if (res == 0) {
			pr_info("Exceed PAGE_SIZE, result trancated\n");
			len += sprintf(buf + len, "\n");
			break;
		}
	}
	spin_unlock_bh(&fpdl->list_lock);

	return len;
}

/**
 * clear statistics (all fastpath devices)
 * 0 - clear fast stats only
 * 1 - clear slow & fast stats
 */
static ssize_t stats_store(struct fastpath_module *m, const char *buf,
			    size_t count)
{
	struct fp_net_device *itr;
	struct net_device_stats *stats_slow;
	struct fp_net_device_stats *stats_fast;
	unsigned int op;
	struct fp_dev_list *fpdl = fp_device->priv;

	if (sscanf(buf, "%u", &op) != 1 || op > 1)
		return -EINVAL;

	spin_lock_bh(&fpdl->list_lock);
	list_for_each_entry(itr, &fpdl->devices_list, list) {
		BUG_ON(!itr->dev);
		if (!fpdev_hold(itr))
			continue;
		stats_slow = &itr->dev->stats;
		stats_fast = &itr->stats;
		if (itr->dev->netdev_ops && itr->dev->netdev_ops->ndo_get_stats)
			stats_slow = itr->dev->netdev_ops->ndo_get_stats(itr->dev);

		memset(stats_fast,0,sizeof(struct fp_net_device_stats));
		if (op)
			memset(stats_slow,0,sizeof(struct net_device_stats));

		fpdev_put(itr);
	}
	spin_unlock_bh(&fpdl->list_lock);

	return count;
}

/**
 * show status (all fastpath devices)
 */
static ssize_t status_show(struct fastpath_module *m, char *buf)
{
	struct fp_net_device *itr;
	struct fp_dev_list *fpdl = fp_device->priv;
	int len = 0;

	len = scnprintf(buf, PAGE_SIZE, "          device   state    forward   refcnt   dev_ref   bridge\n");

	/* active devices */
	rcu_read_lock_bh();
	list_for_each_entry_rcu(itr, &fpdl->devices_list, list)
		len += add_status_to_buff(buf + len, itr, PAGE_SIZE - len -1);
	rcu_read_unlock_bh();

	return len;
}

static void fp_device_release(struct kobject *kobj)
{
	struct fp_dev_list *fpdl = fp_device->priv;
	BUG_ON(!list_empty(&fpdl->devices_list));
	pr_debug("fp_device released\n");
}

static FP_ATTR(devices, S_IRUGO, status_show, NULL);
static FP_ATTR(stats, S_IRUGO|S_IWUSR, stats_show, stats_store);

static struct attribute *fp_device_attrs[] = {
	&fp_attr_devices.attr,
	&fp_attr_stats.attr,
	NULL, /* need to NULL terminate the list of attributes */
};

static struct kobj_type ktype_devices = {
	.sysfs_ops	= &fp_sysfs_ops,
	.default_attrs	= fp_device_attrs,
	.release	= fp_device_release,
};

static int fp_device_probe(struct fastpath_module *module)
{
	int ret;
	struct fp_dev_list *fpdl;

	snprintf(module->name, sizeof(module->name),"fp_device");

	fpdl = kzalloc(sizeof(*fpdl), GFP_KERNEL);
	if (!fpdl) {
		pr_err("fp_dev_list alloc failed\n");
		return -ENOMEM;
	}

	kobject_init(&module->kobj, &ktype_devices);
	ret = kobject_add(&module->kobj, module->fastpath->kobj, "%s", module->name);
	if (ret < 0) {
		pr_err("kobject_add failed (%d)\n", ret);
		goto kobj_err;
	}

	atomic_set(&fpdl->dev_count, 0);
	INIT_LIST_HEAD(&fpdl->devices_list);
	spin_lock_init(&fpdl->list_lock);
	init_waitqueue_head(&fpdl->wq);

	fpdl->dev_put_wq = create_singlethread_workqueue(module->name);
	if (!fpdl->dev_put_wq) {
		pr_err("create workqueue failed\n");
		ret = -EBUSY;
		goto kobj_err;
	}

	module->priv = fpdl;
	fp_device = module;

	kobject_uevent(&module->kobj, KOBJ_ADD);

	pr_debug("fp_device probed\n");
	return 0;

kobj_err:
	kobject_put(&module->kobj);
	kfree(fpdl);
	return ret;
}

static int fp_device_remove(struct fastpath_module *module)
{
	struct fp_dev_list *fpdl = fp_device->priv;

	BUG_ON(!module);

	flush_workqueue(fpdl->dev_put_wq);
	wait_event(fpdl->wq, !atomic_read(&fpdl->dev_count));
	destroy_workqueue(fpdl->dev_put_wq);

	kobject_put(&module->kobj);
	fp_device = NULL;

	kfree(module->priv);
	kfree(module);

	pr_debug("fp_device removed\n");
	return 0;
}

struct fastpath_module_ops fp_device_ops = {
	.probe = fp_device_probe,
	.remove = fp_device_remove,
};