[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / sched / sch_teql.c

/* net/sched/sch_teql.c	"True" (or "trivial") link equalizer.
 *
 *		This program is free software; you can redistribute it and/or
 *		modify it under the terms of the GNU General Public License
 *		as published by the Free Software Foundation; either version
 *		2 of the License, or (at your option) any later version.
 *
 * 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 neighbour *ncache;
	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 net_device *dev = qdisc_dev(sch);
	struct teql_sched_data *q = qdisc_priv(sch);

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

	kfree_skb(skb);
	sch->qstats.drops++;
	return NET_XMIT_DROP;
}

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;

	skb = __skb_dequeue(&dat->q);
	dat_queue = netdev_get_tx_queue(dat->m->dev, 0);
	if (skb == NULL) {
		struct net_device *m = qdisc_dev(dat_queue->qdisc);
		if (m) {
			dat->m->slaves = sch;
			netif_wake_queue(m);
		}
	} else {
		qdisc_bstats_update(sch, skb);
	}
	sch->q.qlen = dat->q.qlen + dat_queue->qdisc->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 inline void
teql_neigh_release(struct neighbour *n)
{
	if (n)
		neigh_release(n);
}

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

	skb_queue_purge(&dat->q);
	sch->q.qlen = 0;
	teql_neigh_release(xchg(&dat->ncache, NULL));
}

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;

	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(txq->qdisc);
						spin_lock_bh(root_lock);
						qdisc_reset(txq->qdisc);
						spin_unlock_bh(root_lock);
					}
				}
				skb_queue_purge(&dat->q);
				teql_neigh_release(xchg(&dat->ncache, NULL));
				break;
			}

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

static int teql_qdisc_init(struct Qdisc *sch, struct nlattr *opt)
{
	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 neighbour *mn)
{
	struct teql_sched_data *q = qdisc_priv(txq->qdisc);
	struct neighbour *n = q->ncache;

	if (mn->tbl == NULL)
		return -EINVAL;
	if (n && n->tbl == mn->tbl &&
	    memcmp(n->primary_key, mn->primary_key, mn->tbl->key_len) == 0) {
		atomic_inc(&n->refcnt);
	} else {
		n = __neigh_lookup_errno(mn->tbl, mn->primary_key, dev);
		if (IS_ERR(n))
			return PTR_ERR(n);
	}
	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), haddr,
				      NULL, skb->len);

		if (err < 0) {
			neigh_release(n);
			return -EINVAL;
		}
		teql_neigh_release(xchg(&q->ncache, n));
		return 0;
	}
	neigh_release(n);
	return (skb_res == NULL) ? -EAGAIN : 1;
}

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);
	struct neighbour *mn;
	int res;

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

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

	rcu_read_lock();
	mn = dst_get_neighbour(dst);
	res = mn ? __teql_resolve(skb, skb_res, dev, txq, mn) : 0;
	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);
		const struct net_device_ops *slave_ops = slave->netdev_ops;

		if (slave_txq->qdisc_sleeping != q)
			continue;
		if (__netif_subqueue_stopped(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_tx_queue_frozen_or_stopped(slave_txq) &&
				    slave_ops->ndo_start_xmit(skb, slave) == NETDEV_TX_OK) {
					txq_trans_update(slave_txq);
					__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_queue_stopped(dev))
				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 struct rtnl_link_stats64 *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;
	return stats;
}

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

	if (new_mtu < 68)
		return -EINVAL;

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

	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->tx_queue_len	= 100;
	dev->flags		= IFF_NOARP;
	dev->hard_header_len	= LL_MAX_HEADER;
	dev->priv_flags		&= ~IFF_XMIT_DST_RELEASE;
}

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", teql_master_setup);
		if (!dev) {
			err = -ENOMEM;
			break;
		}

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

		master = netdev_priv(dev);

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