[linux-2.6-block.git] / net / sched / sch_teql.c

// SPDX-License-Identifier: GPL-2.0-or-later
/* net/sched/sch_teql.c	"True" (or "trivial") link equalizer.
 *
 * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/if_arp.h>
#include <linux/netdevice.h>
#include <linux/init.h>
#include <linux/skbuff.h>
#include <linux/moduleparam.h>
#include <net/dst.h>
#include <net/neighbour.h>
#include <net/pkt_sched.h>

/*
   How to setup it.
   ----------------

   After loading this module you will find a new device teqlN
   and new qdisc with the same name. To join a slave to the equalizer
   you should just set this qdisc on a device f.e.

   # tc qdisc add dev eth0 root teql0
   # tc qdisc add dev eth1 root teql0

   That's all. Full PnP 8)

   Applicability.
   --------------

   1. Slave devices MUST be active devices, i.e., they must raise the tbusy
      signal and generate EOI events. If you want to equalize virtual devices
      like tunnels, use a normal eql device.
   2. This device puts no limitations on physical slave characteristics
      f.e. it will equalize 9600baud line and 100Mb ethernet perfectly :-)
      Certainly, large difference in link speeds will make the resulting
      eqalized link unusable, because of huge packet reordering.
      I estimate an upper useful difference as ~10 times.
   3. If the slave requires address resolution, only protocols using
      neighbour cache (IPv4/IPv6) will work over the equalized link.
      Other protocols are still allowed to use the slave device directly,
      which will not break load balancing, though native slave
      traffic will have the highest priority.  */

struct teql_master {
	struct Qdisc_ops qops;
	struct net_device *dev;
	struct Qdisc *slaves;
	struct list_head master_list;
	unsigned long	tx_bytes;
	unsigned long	tx_packets;
	unsigned long	tx_errors;
	unsigned long	tx_dropped;
};

struct teql_sched_data {
	struct Qdisc *next;
	struct teql_master *m;
	struct sk_buff_head q;
};

#define NEXT_SLAVE(q) (((struct teql_sched_data *)qdisc_priv(q))->next)

#define FMASK (IFF_BROADCAST | IFF_POINTOPOINT)

/* "teql*" qdisc routines */

static int
teql_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
{
	struct net_device *dev = qdisc_dev(sch);
	struct teql_sched_data *q = qdisc_priv(sch);

	if (q->q.qlen < READ_ONCE(dev->tx_queue_len)) {
		__skb_queue_tail(&q->q, skb);
		return NET_XMIT_SUCCESS;
	}

	return qdisc_drop(skb, sch, to_free);
}

static struct sk_buff *
teql_dequeue(struct Qdisc *sch)
{
	struct teql_sched_data *dat = qdisc_priv(sch);
	struct netdev_queue *dat_queue;
	struct sk_buff *skb;
	struct Qdisc *q;

	skb = __skb_dequeue(&dat->q);
	dat_queue = netdev_get_tx_queue(dat->m->dev, 0);
	q = rcu_dereference_bh(dat_queue->qdisc);

	if (skb == NULL) {
		struct net_device *m = qdisc_dev(q);
		if (m) {
			dat->m->slaves = sch;
			netif_wake_queue(m);
		}
	} else {
		qdisc_bstats_update(sch, skb);
	}
	sch->q.qlen = dat->q.qlen + q->q.qlen;
	return skb;
}

static struct sk_buff *
teql_peek(struct Qdisc *sch)
{
	/* teql is meant to be used as root qdisc */
	return NULL;
}

static void
teql_reset(struct Qdisc *sch)
{
	struct teql_sched_data *dat = qdisc_priv(sch);

	skb_queue_purge(&dat->q);
}

static void
teql_destroy(struct Qdisc *sch)
{
	struct Qdisc *q, *prev;
	struct teql_sched_data *dat = qdisc_priv(sch);
	struct teql_master *master = dat->m;

	if (!master)
		return;

	prev = master->slaves;
	if (prev) {
		do {
			q = NEXT_SLAVE(prev);
			if (q == sch) {
				NEXT_SLAVE(prev) = NEXT_SLAVE(q);
				if (q == master->slaves) {
					master->slaves = NEXT_SLAVE(q);
					if (q == master->slaves) {
						struct netdev_queue *txq;
						spinlock_t *root_lock;

						txq = netdev_get_tx_queue(master->dev, 0);
						master->slaves = NULL;

						root_lock = qdisc_root_sleeping_lock(rtnl_dereference(txq->qdisc));
						spin_lock_bh(root_lock);
						qdisc_reset(rtnl_dereference(txq->qdisc));
						spin_unlock_bh(root_lock);
					}
				}
				skb_queue_purge(&dat->q);
				break;
			}

		} while ((prev = q) != master->slaves);
	}
}

static int teql_qdisc_init(struct Qdisc *sch, struct nlattr *opt,
			   struct netlink_ext_ack *extack)
{
	struct net_device *dev = qdisc_dev(sch);
	struct teql_master *m = (struct teql_master *)sch->ops;
	struct teql_sched_data *q = qdisc_priv(sch);

	if (dev->hard_header_len > m->dev->hard_header_len)
		return -EINVAL;

	if (m->dev == dev)
		return -ELOOP;

	q->m = m;

	skb_queue_head_init(&q->q);

	if (m->slaves) {
		if (m->dev->flags & IFF_UP) {
			if ((m->dev->flags & IFF_POINTOPOINT &&
			     !(dev->flags & IFF_POINTOPOINT)) ||
			    (m->dev->flags & IFF_BROADCAST &&
			     !(dev->flags & IFF_BROADCAST)) ||
			    (m->dev->flags & IFF_MULTICAST &&
			     !(dev->flags & IFF_MULTICAST)) ||
			    dev->mtu < m->dev->mtu)
				return -EINVAL;
		} else {
			if (!(dev->flags&IFF_POINTOPOINT))
				m->dev->flags &= ~IFF_POINTOPOINT;
			if (!(dev->flags&IFF_BROADCAST))
				m->dev->flags &= ~IFF_BROADCAST;
			if (!(dev->flags&IFF_MULTICAST))
				m->dev->flags &= ~IFF_MULTICAST;
			if (dev->mtu < m->dev->mtu)
				m->dev->mtu = dev->mtu;
		}
		q->next = NEXT_SLAVE(m->slaves);
		NEXT_SLAVE(m->slaves) = sch;
	} else {
		q->next = sch;
		m->slaves = sch;
		m->dev->mtu = dev->mtu;
		m->dev->flags = (m->dev->flags&~FMASK)|(dev->flags&FMASK);
	}
	return 0;
}


static int
__teql_resolve(struct sk_buff *skb, struct sk_buff *skb_res,
	       struct net_device *dev, struct netdev_queue *txq,
	       struct dst_entry *dst)
{
	struct neighbour *n;
	int err = 0;

	n = dst_neigh_lookup_skb(dst, skb);
	if (!n)
		return -ENOENT;

	if (dst->dev != dev) {
		struct neighbour *mn;

		mn = __neigh_lookup_errno(n->tbl, n->primary_key, dev);
		neigh_release(n);
		if (IS_ERR(mn))
			return PTR_ERR(mn);
		n = mn;
	}

	if (neigh_event_send(n, skb_res) == 0) {
		int err;
		char haddr[MAX_ADDR_LEN];

		neigh_ha_snapshot(haddr, n, dev);
		err = dev_hard_header(skb, dev, ntohs(skb_protocol(skb, false)),
				      haddr, NULL, skb->len);

		if (err < 0)
			err = -EINVAL;
	} else {
		err = (skb_res == NULL) ? -EAGAIN : 1;
	}
	neigh_release(n);
	return err;
}

static inline int teql_resolve(struct sk_buff *skb,
			       struct sk_buff *skb_res,
			       struct net_device *dev,
			       struct netdev_queue *txq)
{
	struct dst_entry *dst = skb_dst(skb);
	int res;

	if (rcu_access_pointer(txq->qdisc) == &noop_qdisc)
		return -ENODEV;

	if (!dev->header_ops || !dst)
		return 0;

	rcu_read_lock();
	res = __teql_resolve(skb, skb_res, dev, txq, dst);
	rcu_read_unlock();

	return res;
}

static netdev_tx_t teql_master_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct teql_master *master = netdev_priv(dev);
	struct Qdisc *start, *q;
	int busy;
	int nores;
	int subq = skb_get_queue_mapping(skb);
	struct sk_buff *skb_res = NULL;

	start = master->slaves;

restart:
	nores = 0;
	busy = 0;

	q = start;
	if (!q)
		goto drop;

	do {
		struct net_device *slave = qdisc_dev(q);
		struct netdev_queue *slave_txq = netdev_get_tx_queue(slave, 0);

		if (rcu_access_pointer(slave_txq->qdisc_sleeping) != q)
			continue;
		if (netif_xmit_stopped(netdev_get_tx_queue(slave, subq)) ||
		    !netif_running(slave)) {
			busy = 1;
			continue;
		}

		switch (teql_resolve(skb, skb_res, slave, slave_txq)) {
		case 0:
			if (__netif_tx_trylock(slave_txq)) {
				unsigned int length = qdisc_pkt_len(skb);

				if (!netif_xmit_frozen_or_stopped(slave_txq) &&
				    netdev_start_xmit(skb, slave, slave_txq, false) ==
				    NETDEV_TX_OK) {
					__netif_tx_unlock(slave_txq);
					master->slaves = NEXT_SLAVE(q);
					netif_wake_queue(dev);
					master->tx_packets++;
					master->tx_bytes += length;
					return NETDEV_TX_OK;
				}
				__netif_tx_unlock(slave_txq);
			}
			if (netif_xmit_stopped(netdev_get_tx_queue(dev, 0)))
				busy = 1;
			break;
		case 1:
			master->slaves = NEXT_SLAVE(q);
			return NETDEV_TX_OK;
		default:
			nores = 1;
			break;
		}
		__skb_pull(skb, skb_network_offset(skb));
	} while ((q = NEXT_SLAVE(q)) != start);

	if (nores && skb_res == NULL) {
		skb_res = skb;
		goto restart;
	}

	if (busy) {
		netif_stop_queue(dev);
		return NETDEV_TX_BUSY;
	}
	master->tx_errors++;

drop:
	master->tx_dropped++;
	dev_kfree_skb(skb);
	return NETDEV_TX_OK;
}

static int teql_master_open(struct net_device *dev)
{
	struct Qdisc *q;
	struct teql_master *m = netdev_priv(dev);
	int mtu = 0xFFFE;
	unsigned int flags = IFF_NOARP | IFF_MULTICAST;

	if (m->slaves == NULL)
		return -EUNATCH;

	flags = FMASK;

	q = m->slaves;
	do {
		struct net_device *slave = qdisc_dev(q);

		if (slave == NULL)
			return -EUNATCH;

		if (slave->mtu < mtu)
			mtu = slave->mtu;
		if (slave->hard_header_len > LL_MAX_HEADER)
			return -EINVAL;

		/* If all the slaves are BROADCAST, master is BROADCAST
		   If all the slaves are PtP, master is PtP
		   Otherwise, master is NBMA.
		 */
		if (!(slave->flags&IFF_POINTOPOINT))
			flags &= ~IFF_POINTOPOINT;
		if (!(slave->flags&IFF_BROADCAST))
			flags &= ~IFF_BROADCAST;
		if (!(slave->flags&IFF_MULTICAST))
			flags &= ~IFF_MULTICAST;
	} while ((q = NEXT_SLAVE(q)) != m->slaves);

	m->dev->mtu = mtu;
	m->dev->flags = (m->dev->flags&~FMASK) | flags;
	netif_start_queue(m->dev);
	return 0;
}

static int teql_master_close(struct net_device *dev)
{
	netif_stop_queue(dev);
	return 0;
}

static void teql_master_stats64(struct net_device *dev,
				struct rtnl_link_stats64 *stats)
{
	struct teql_master *m = netdev_priv(dev);

	stats->tx_packets	= m->tx_packets;
	stats->tx_bytes		= m->tx_bytes;
	stats->tx_errors	= m->tx_errors;
	stats->tx_dropped	= m->tx_dropped;
}

static int teql_master_mtu(struct net_device *dev, int new_mtu)
{
	struct teql_master *m = netdev_priv(dev);
	struct Qdisc *q;

	q = m->slaves;
	if (q) {
		do {
			if (new_mtu > qdisc_dev(q)->mtu)
				return -EINVAL;
		} while ((q = NEXT_SLAVE(q)) != m->slaves);
	}

	WRITE_ONCE(dev->mtu, new_mtu);
	return 0;
}

static const struct net_device_ops teql_netdev_ops = {
	.ndo_open	= teql_master_open,
	.ndo_stop	= teql_master_close,
	.ndo_start_xmit	= teql_master_xmit,
	.ndo_get_stats64 = teql_master_stats64,
	.ndo_change_mtu	= teql_master_mtu,
};

static __init void teql_master_setup(struct net_device *dev)
{
	struct teql_master *master = netdev_priv(dev);
	struct Qdisc_ops *ops = &master->qops;

	master->dev	= dev;
	ops->priv_size  = sizeof(struct teql_sched_data);

	ops->enqueue	=	teql_enqueue;
	ops->dequeue	=	teql_dequeue;
	ops->peek	=	teql_peek;
	ops->init	=	teql_qdisc_init;
	ops->reset	=	teql_reset;
	ops->destroy	=	teql_destroy;
	ops->owner	=	THIS_MODULE;

	dev->netdev_ops =       &teql_netdev_ops;
	dev->type		= ARPHRD_VOID;
	dev->mtu		= 1500;
	dev->min_mtu		= 68;
	dev->max_mtu		= 65535;
	dev->tx_queue_len	= 100;
	dev->flags		= IFF_NOARP;
	dev->hard_header_len	= LL_MAX_HEADER;
	netif_keep_dst(dev);
}

static LIST_HEAD(master_dev_list);
static int max_equalizers = 1;
module_param(max_equalizers, int, 0);
MODULE_PARM_DESC(max_equalizers, "Max number of link equalizers");

static int __init teql_init(void)
{
	int i;
	int err = -ENODEV;

	for (i = 0; i < max_equalizers; i++) {
		struct net_device *dev;
		struct teql_master *master;

		dev = alloc_netdev(sizeof(struct teql_master), "teql%d",
				   NET_NAME_UNKNOWN, teql_master_setup);
		if (!dev) {
			err = -ENOMEM;
			break;
		}

		if ((err = register_netdev(dev))) {
			free_netdev(dev);
			break;
		}

		master = netdev_priv(dev);

		strscpy(master->qops.id, dev->name, IFNAMSIZ);
		err = register_qdisc(&master->qops);

		if (err) {
			unregister_netdev(dev);
			free_netdev(dev);
			break;
		}

		list_add_tail(&master->master_list, &master_dev_list);
	}
	return i ? 0 : err;
}

static void __exit teql_exit(void)
{
	struct teql_master *master, *nxt;

	list_for_each_entry_safe(master, nxt, &master_dev_list, master_list) {

		list_del(&master->master_list);

		unregister_qdisc(&master->qops);
		unregister_netdev(master->dev);
		free_netdev(master->dev);
	}
}

module_init(teql_init);
module_exit(teql_exit);

MODULE_LICENSE("GPL");
MODULE_DESCRIPTION("True (or trivial) link equalizer qdisc");
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
1da177e4	2	/* net/sched/sch_teql.c "True" (or "trivial") link equalizer.
1da177e4 LT	3	*
	4	* Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
	5	*/
	6
	7	#include <linux/module.h>
1da177e4 LT	8	#include <linux/types.h>
1da177e4 LT	9	#include <linux/kernel.h>
5a0e3ad6	10	#include <linux/slab.h>
1da177e4	11	#include <linux/string.h>
1da177e4	12	#include <linux/errno.h>
14c85021	13	#include <linux/if_arp.h>
1da177e4	14	#include <linux/netdevice.h>
1da177e4	15	#include <linux/init.h>
1da177e4 LT	16	#include <linux/skbuff.h>
1da177e4 LT	17	#include <linux/moduleparam.h>
0ba48053 PM	18	#include <net/dst.h>
0ba48053 PM	19	#include <net/neighbour.h>
1da177e4 LT	20	#include <net/pkt_sched.h>
	21
	22	/*
	23	How to setup it.
	24	----------------
	25
	26	After loading this module you will find a new device teqlN
	27	and new qdisc with the same name. To join a slave to the equalizer
	28	you should just set this qdisc on a device f.e.
	29
	30	# tc qdisc add dev eth0 root teql0
	31	# tc qdisc add dev eth1 root teql0
	32
	33	That's all. Full PnP 8)
	34
	35	Applicability.
	36	--------------
	37
	38	1. Slave devices MUST be active devices, i.e., they must raise the tbusy
	39	signal and generate EOI events. If you want to equalize virtual devices
	40	like tunnels, use a normal eql device.
	41	2. This device puts no limitations on physical slave characteristics
	42	f.e. it will equalize 9600baud line and 100Mb ethernet perfectly :-)
	43	Certainly, large difference in link speeds will make the resulting
	44	eqalized link unusable, because of huge packet reordering.
	45	I estimate an upper useful difference as ~10 times.
	46	3. If the slave requires address resolution, only protocols using
	47	neighbour cache (IPv4/IPv6) will work over the equalized link.
	48	Other protocols are still allowed to use the slave device directly,
	49	which will not break load balancing, though native slave
	50	traffic will have the highest priority. */
	51
cc7ec456	52	struct teql_master {
1da177e4 LT	53	struct Qdisc_ops qops;
	54	struct net_device *dev;
	55	struct Qdisc *slaves;
	56	struct list_head master_list;
1ac9ad13 ED	57	unsigned long tx_bytes;
	58	unsigned long tx_packets;
	59	unsigned long tx_errors;
	60	unsigned long tx_dropped;
1da177e4 LT	61	};
1da177e4 LT	62
cc7ec456	63	struct teql_sched_data {
1da177e4 LT	64	struct Qdisc *next;
1da177e4 LT	65	struct teql_master *m;
1da177e4 LT	66	struct sk_buff_head q;
	67	};
	68
cc7ec456	69	#define NEXT_SLAVE(q) (((struct teql_sched_data *)qdisc_priv(q))->next)
1da177e4	70
cc7ec456	71	#define FMASK (IFF_BROADCAST \| IFF_POINTOPOINT)
1da177e4 LT	72
	73	/* "teql" qdisc routines /
	74
	75	static int
520ac30f	76	teql_enqueue(struct sk_buff skb, struct Qdisc sch, struct sk_buff **to_free)
1da177e4	77	{
5ce2d488	78	struct net_device *dev = qdisc_dev(sch);
1da177e4 LT	79	struct teql_sched_data *q = qdisc_priv(sch);
1da177e4 LT	80
ad13b5b0	81	if (q->q.qlen < READ_ONCE(dev->tx_queue_len)) {
4cd8c9e8	82	__skb_queue_tail(&q->q, skb);
9871e50e	83	return NET_XMIT_SUCCESS;
1da177e4 LT	84	}
1da177e4 LT	85
520ac30f	86	return qdisc_drop(skb, sch, to_free);
1da177e4 LT	87	}
1da177e4 LT	88
1da177e4	89	static struct sk_buff *
cc7ec456	90	teql_dequeue(struct Qdisc *sch)
1da177e4 LT	91	{
1da177e4 LT	92	struct teql_sched_data *dat = qdisc_priv(sch);
b0e1e646	93	struct netdev_queue *dat_queue;
1da177e4	94	struct sk_buff *skb;
46e5da40	95	struct Qdisc *q;
1da177e4 LT	96
1da177e4 LT	97	skb = __skb_dequeue(&dat->q);
e8a0464c	98	dat_queue = netdev_get_tx_queue(dat->m->dev, 0);
46e5da40 JF	99	q = rcu_dereference_bh(dat_queue->qdisc);
46e5da40 JF	100
1da177e4	101	if (skb == NULL) {
46e5da40	102	struct net_device *m = qdisc_dev(q);
1da177e4 LT	103	if (m) {
	104	dat->m->slaves = sch;
	105	netif_wake_queue(m);
	106	}
9190b3b3 ED	107	} else {
9190b3b3 ED	108	qdisc_bstats_update(sch, skb);
1da177e4	109	}
46e5da40	110	sch->q.qlen = dat->q.qlen + q->q.qlen;
1da177e4 LT	111	return skb;
	112	}
	113
8e3af978	114	static struct sk_buff *
cc7ec456	115	teql_peek(struct Qdisc *sch)
8e3af978 JP	116	{
	117	/* teql is meant to be used as root qdisc */
	118	return NULL;
	119	}
	120
1da177e4	121	static void
cc7ec456	122	teql_reset(struct Qdisc *sch)
1da177e4 LT	123	{
	124	struct teql_sched_data *dat = qdisc_priv(sch);
	125
	126	skb_queue_purge(&dat->q);
1da177e4 LT	127	}
	128
	129	static void
cc7ec456	130	teql_destroy(struct Qdisc *sch)
1da177e4 LT	131	{
	132	struct Qdisc q, prev;
	133	struct teql_sched_data *dat = qdisc_priv(sch);
	134	struct teql_master *master = dat->m;
	135
1ffbc7ea PT	136	if (!master)
	137	return;
	138
cc7ec456 ED	139	prev = master->slaves;
cc7ec456 ED	140	if (prev) {
1da177e4 LT	141	do {
	142	q = NEXT_SLAVE(prev);
	143	if (q == sch) {
	144	NEXT_SLAVE(prev) = NEXT_SLAVE(q);
	145	if (q == master->slaves) {
	146	master->slaves = NEXT_SLAVE(q);
	147	if (q == master->slaves) {
e8a0464c	148	struct netdev_queue *txq;
83874000	149	spinlock_t *root_lock;
e8a0464c DM	150
e8a0464c DM	151	txq = netdev_get_tx_queue(master->dev, 0);
1da177e4	152	master->slaves = NULL;
83874000	153
46e5da40	154	root_lock = qdisc_root_sleeping_lock(rtnl_dereference(txq->qdisc));
83874000	155	spin_lock_bh(root_lock);
46e5da40	156	qdisc_reset(rtnl_dereference(txq->qdisc));
83874000	157	spin_unlock_bh(root_lock);
1da177e4 LT	158	}
	159	}
	160	skb_queue_purge(&dat->q);
1da177e4 LT	161	break;
1da177e4 LT	162	}
10297b99	163
1da177e4 LT	164	} while ((prev = q) != master->slaves);
	165	}
	166	}
	167
e63d7dfd AA	168	static int teql_qdisc_init(struct Qdisc sch, struct nlattr opt,
e63d7dfd AA	169	struct netlink_ext_ack *extack)
1da177e4	170	{
5ce2d488	171	struct net_device *dev = qdisc_dev(sch);
cc7ec456	172	struct teql_master m = (struct teql_master )sch->ops;
1da177e4 LT	173	struct teql_sched_data *q = qdisc_priv(sch);
	174
	175	if (dev->hard_header_len > m->dev->hard_header_len)
	176	return -EINVAL;
	177
	178	if (m->dev == dev)
	179	return -ELOOP;
	180
	181	q->m = m;
	182
	183	skb_queue_head_init(&q->q);
	184
	185	if (m->slaves) {
	186	if (m->dev->flags & IFF_UP) {
f64f9e71 JP	187	if ((m->dev->flags & IFF_POINTOPOINT &&
	188	!(dev->flags & IFF_POINTOPOINT)) \|\|
	189	(m->dev->flags & IFF_BROADCAST &&
	190	!(dev->flags & IFF_BROADCAST)) \|\|
	191	(m->dev->flags & IFF_MULTICAST &&
	192	!(dev->flags & IFF_MULTICAST)) \|\|
	193	dev->mtu < m->dev->mtu)
1da177e4 LT	194	return -EINVAL;
	195	} else {
	196	if (!(dev->flags&IFF_POINTOPOINT))
	197	m->dev->flags &= ~IFF_POINTOPOINT;
	198	if (!(dev->flags&IFF_BROADCAST))
	199	m->dev->flags &= ~IFF_BROADCAST;
	200	if (!(dev->flags&IFF_MULTICAST))
	201	m->dev->flags &= ~IFF_MULTICAST;
	202	if (dev->mtu < m->dev->mtu)
	203	m->dev->mtu = dev->mtu;
	204	}
	205	q->next = NEXT_SLAVE(m->slaves);
	206	NEXT_SLAVE(m->slaves) = sch;
	207	} else {
	208	q->next = sch;
	209	m->slaves = sch;
	210	m->dev->mtu = dev->mtu;
	211	m->dev->flags = (m->dev->flags&~FMASK)\|(dev->flags&FMASK);
	212	}
	213	return 0;
	214	}
	215
1da177e4 LT	216
1da177e4 LT	217	static int
f7e57044 ED	218	__teql_resolve(struct sk_buff skb, struct sk_buff skb_res,
f7e57044 ED	219	struct net_device dev, struct netdev_queue txq,
dbedbe6d	220	struct dst_entry *dst)
1da177e4	221	{
dbedbe6d DM	222	struct neighbour *n;
dbedbe6d DM	223	int err = 0;
1da177e4	224
dbedbe6d DM	225	n = dst_neigh_lookup_skb(dst, skb);
	226	if (!n)
	227	return -ENOENT;
	228
	229	if (dst->dev != dev) {
	230	struct neighbour *mn;
	231
	232	mn = __neigh_lookup_errno(n->tbl, n->primary_key, dev);
	233	neigh_release(n);
	234	if (IS_ERR(mn))
	235	return PTR_ERR(mn);
	236	n = mn;
1da177e4	237	}
dbedbe6d	238
1da177e4 LT	239	if (neigh_event_send(n, skb_res) == 0) {
1da177e4 LT	240	int err;
0ed8ddf4	241	char haddr[MAX_ADDR_LEN];
0c4e8581	242
0ed8ddf4	243	neigh_ha_snapshot(haddr, n, dev);
d7bf2ebe	244	err = dev_hard_header(skb, dev, ntohs(skb_protocol(skb, false)),
d8b9605d	245	haddr, NULL, skb->len);
0c4e8581	246
dbedbe6d DM	247	if (err < 0)
	248	err = -EINVAL;
	249	} else {
	250	err = (skb_res == NULL) ? -EAGAIN : 1;
1da177e4 LT	251	}
1da177e4 LT	252	neigh_release(n);
dbedbe6d	253	return err;
1da177e4 LT	254	}
1da177e4 LT	255
3b04ddde	256	static inline int teql_resolve(struct sk_buff *skb,
f7e57044 ED	257	struct sk_buff *skb_res,
	258	struct net_device *dev,
	259	struct netdev_queue *txq)
1da177e4	260	{
f7e57044	261	struct dst_entry *dst = skb_dst(skb);
f7e57044 ED	262	int res;
f7e57044 ED	263
46e5da40	264	if (rcu_access_pointer(txq->qdisc) == &noop_qdisc)
4f9f8311 EP	265	return -ENODEV;
4f9f8311 EP	266
f7e57044	267	if (!dev->header_ops \|\| !dst)
1da177e4	268	return 0;
f7e57044 ED	269
f7e57044 ED	270	rcu_read_lock();
dbedbe6d	271	res = __teql_resolve(skb, skb_res, dev, txq, dst);
f7e57044 ED	272	rcu_read_unlock();
	273
	274	return res;
1da177e4 LT	275	}
1da177e4 LT	276
6fef4c0c	277	static netdev_tx_t teql_master_xmit(struct sk_buff skb, struct net_device dev)
1da177e4	278	{
2941a486	279	struct teql_master *master = netdev_priv(dev);
1da177e4 LT	280	struct Qdisc start, q;
	281	int busy;
	282	int nores;
4e3ab47a	283	int subq = skb_get_queue_mapping(skb);
1da177e4 LT	284	struct sk_buff *skb_res = NULL;
	285
	286	start = master->slaves;
	287
	288	restart:
	289	nores = 0;
	290	busy = 0;
	291
cc7ec456 ED	292	q = start;
cc7ec456 ED	293	if (!q)
1da177e4 LT	294	goto drop;
	295
	296	do {
5ce2d488	297	struct net_device *slave = qdisc_dev(q);
61294e2e	298	struct netdev_queue *slave_txq = netdev_get_tx_queue(slave, 0);
10297b99	299
d636fc5d	300	if (rcu_access_pointer(slave_txq->qdisc_sleeping) != q)
1da177e4	301	continue;
73466498	302	if (netif_xmit_stopped(netdev_get_tx_queue(slave, subq)) \|\|
f25f4e44	303	!netif_running(slave)) {
1da177e4 LT	304	busy = 1;
	305	continue;
	306	}
	307
f7e57044	308	switch (teql_resolve(skb, skb_res, slave, slave_txq)) {
1da177e4	309	case 0:
c3f26a26	310	if (__netif_tx_trylock(slave_txq)) {
c0f84d0d ED	311	unsigned int length = qdisc_pkt_len(skb);
c0f84d0d ED	312
73466498	313	if (!netif_xmit_frozen_or_stopped(slave_txq) &&
fa2dbdc2 DM	314	netdev_start_xmit(skb, slave, slave_txq, false) ==
fa2dbdc2 DM	315	NETDEV_TX_OK) {
c3f26a26	316	__netif_tx_unlock(slave_txq);
1da177e4 LT	317	master->slaves = NEXT_SLAVE(q);
1da177e4 LT	318	netif_wake_queue(dev);
1ac9ad13 ED	319	master->tx_packets++;
1ac9ad13 ED	320	master->tx_bytes += length;
6ed10654	321	return NETDEV_TX_OK;
1da177e4	322	}
c3f26a26	323	__netif_tx_unlock(slave_txq);
1da177e4	324	}
73466498	325	if (netif_xmit_stopped(netdev_get_tx_queue(dev, 0)))
1da177e4 LT	326	busy = 1;
	327	break;
	328	case 1:
	329	master->slaves = NEXT_SLAVE(q);
6ed10654	330	return NETDEV_TX_OK;
1da177e4 LT	331	default:
	332	nores = 1;
	333	break;
	334	}
bbe735e4	335	__skb_pull(skb, skb_network_offset(skb));
1da177e4 LT	336	} while ((q = NEXT_SLAVE(q)) != start);
	337
	338	if (nores && skb_res == NULL) {
	339	skb_res = skb;
	340	goto restart;
	341	}
	342
	343	if (busy) {
	344	netif_stop_queue(dev);
5b548140	345	return NETDEV_TX_BUSY;
1da177e4	346	}
1ac9ad13	347	master->tx_errors++;
1da177e4 LT	348
1da177e4 LT	349	drop:
1ac9ad13	350	master->tx_dropped++;
1da177e4	351	dev_kfree_skb(skb);
6ed10654	352	return NETDEV_TX_OK;
1da177e4 LT	353	}
	354
	355	static int teql_master_open(struct net_device *dev)
	356	{
cc7ec456	357	struct Qdisc *q;
2941a486	358	struct teql_master *m = netdev_priv(dev);
1da177e4	359	int mtu = 0xFFFE;
cc7ec456	360	unsigned int flags = IFF_NOARP \| IFF_MULTICAST;
1da177e4 LT	361
	362	if (m->slaves == NULL)
	363	return -EUNATCH;
	364
	365	flags = FMASK;
	366
	367	q = m->slaves;
	368	do {
5ce2d488	369	struct net_device *slave = qdisc_dev(q);
1da177e4 LT	370
	371	if (slave == NULL)
	372	return -EUNATCH;
	373
	374	if (slave->mtu < mtu)
	375	mtu = slave->mtu;
	376	if (slave->hard_header_len > LL_MAX_HEADER)
	377	return -EINVAL;
	378
	379	/* If all the slaves are BROADCAST, master is BROADCAST
	380	If all the slaves are PtP, master is PtP
	381	Otherwise, master is NBMA.
	382	*/
	383	if (!(slave->flags&IFF_POINTOPOINT))
	384	flags &= ~IFF_POINTOPOINT;
	385	if (!(slave->flags&IFF_BROADCAST))
	386	flags &= ~IFF_BROADCAST;
	387	if (!(slave->flags&IFF_MULTICAST))
	388	flags &= ~IFF_MULTICAST;
	389	} while ((q = NEXT_SLAVE(q)) != m->slaves);
	390
	391	m->dev->mtu = mtu;
	392	m->dev->flags = (m->dev->flags&~FMASK) \| flags;
	393	netif_start_queue(m->dev);
	394	return 0;
	395	}
	396
	397	static int teql_master_close(struct net_device *dev)
	398	{
	399	netif_stop_queue(dev);
	400	return 0;
	401	}
	402
bc1f4470	403	static void teql_master_stats64(struct net_device *dev,
bc1f4470	404	struct rtnl_link_stats64 *stats)
1ac9ad13 ED	405	{
	406	struct teql_master *m = netdev_priv(dev);
	407
	408	stats->tx_packets = m->tx_packets;
	409	stats->tx_bytes = m->tx_bytes;
	410	stats->tx_errors = m->tx_errors;
	411	stats->tx_dropped = m->tx_dropped;
1ac9ad13 ED	412	}
1ac9ad13 ED	413
1da177e4 LT	414	static int teql_master_mtu(struct net_device *dev, int new_mtu)
1da177e4 LT	415	{
2941a486	416	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	417	struct Qdisc *q;
1da177e4 LT	418
1da177e4 LT	419	q = m->slaves;
	420	if (q) {
	421	do {
5ce2d488	422	if (new_mtu > qdisc_dev(q)->mtu)
1da177e4	423	return -EINVAL;
cc7ec456	424	} while ((q = NEXT_SLAVE(q)) != m->slaves);
1da177e4 LT	425	}
1da177e4 LT	426
1eb2cded	427	WRITE_ONCE(dev->mtu, new_mtu);
1da177e4 LT	428	return 0;
	429	}
	430
61294e2e SH	431	static const struct net_device_ops teql_netdev_ops = {
	432	.ndo_open = teql_master_open,
	433	.ndo_stop = teql_master_close,
	434	.ndo_start_xmit = teql_master_xmit,
1ac9ad13	435	.ndo_get_stats64 = teql_master_stats64,
61294e2e SH	436	.ndo_change_mtu = teql_master_mtu,
	437	};
	438
1da177e4 LT	439	static __init void teql_master_setup(struct net_device *dev)
1da177e4 LT	440	{
2941a486	441	struct teql_master *master = netdev_priv(dev);
1da177e4 LT	442	struct Qdisc_ops *ops = &master->qops;
	443
	444	master->dev = dev;
	445	ops->priv_size = sizeof(struct teql_sched_data);
10297b99	446
1da177e4 LT	447	ops->enqueue = teql_enqueue;
1da177e4 LT	448	ops->dequeue = teql_dequeue;
8e3af978	449	ops->peek = teql_peek;
1da177e4 LT	450	ops->init = teql_qdisc_init;
	451	ops->reset = teql_reset;
	452	ops->destroy = teql_destroy;
	453	ops->owner = THIS_MODULE;
	454
61294e2e	455	dev->netdev_ops = &teql_netdev_ops;
1da177e4 LT	456	dev->type = ARPHRD_VOID;
1da177e4 LT	457	dev->mtu = 1500;
91572088 JW	458	dev->min_mtu = 68;
91572088 JW	459	dev->max_mtu = 65535;
1da177e4 LT	460	dev->tx_queue_len = 100;
	461	dev->flags = IFF_NOARP;
	462	dev->hard_header_len = LL_MAX_HEADER;
02875878	463	netif_keep_dst(dev);
1da177e4 LT	464	}
	465
	466	static LIST_HEAD(master_dev_list);
	467	static int max_equalizers = 1;
	468	module_param(max_equalizers, int, 0);
	469	MODULE_PARM_DESC(max_equalizers, "Max number of link equalizers");
	470
	471	static int __init teql_init(void)
	472	{
	473	int i;
	474	int err = -ENODEV;
	475
	476	for (i = 0; i < max_equalizers; i++) {
	477	struct net_device *dev;
	478	struct teql_master *master;
	479
c835a677 TG	480	dev = alloc_netdev(sizeof(struct teql_master), "teql%d",
c835a677 TG	481	NET_NAME_UNKNOWN, teql_master_setup);
1da177e4 LT	482	if (!dev) {
	483	err = -ENOMEM;
	484	break;
	485	}
	486
	487	if ((err = register_netdev(dev))) {
	488	free_netdev(dev);
	489	break;
	490	}
	491
2941a486	492	master = netdev_priv(dev);
1da177e4	493
92f24c6f	494	strscpy(master->qops.id, dev->name, IFNAMSIZ);
1da177e4 LT	495	err = register_qdisc(&master->qops);
	496
	497	if (err) {
	498	unregister_netdev(dev);
	499	free_netdev(dev);
	500	break;
	501	}
	502
	503	list_add_tail(&master->master_list, &master_dev_list);
	504	}
	505	return i ? 0 : err;
	506	}
	507
10297b99	508	static void __exit teql_exit(void)
1da177e4 LT	509	{
	510	struct teql_master master, nxt;
	511
	512	list_for_each_entry_safe(master, nxt, &master_dev_list, master_list) {
	513
	514	list_del(&master->master_list);
	515
	516	unregister_qdisc(&master->qops);
	517	unregister_netdev(master->dev);
	518	free_netdev(master->dev);
	519	}
	520	}
	521
	522	module_init(teql_init);
	523	module_exit(teql_exit);
	524
	525	MODULE_LICENSE("GPL");
f96118c5	526	MODULE_DESCRIPTION("True (or trivial) link equalizer qdisc");