[net-next-2.6.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 <asm/uaccess.h>
#include <asm/system.h>
#include <linux/bitops.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/mm.h>
#include <linux/socket.h>
#include <linux/sockios.h>
#include <linux/in.h>
#include <linux/errno.h>
#include <linux/interrupt.h>
#include <linux/if_arp.h>
#include <linux/if_ether.h>
#include <linux/inet.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/notifier.h>
#include <linux/init.h>
#include <net/ip.h>
#include <net/route.h>
#include <linux/skbuff.h>
#include <linux/moduleparam.h>
#include <net/sock.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;
	struct net_device_stats stats;
};

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

/* "teql*" qdisc routines */

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

	if (q->q.qlen < dev->tx_queue_len) {
		__skb_queue_tail(&q->q, skb);
		sch->bstats.bytes += skb->len;
		sch->bstats.packets++;
		return 0;
	}

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

static int
teql_requeue(struct sk_buff *skb, struct Qdisc* sch)
{
	struct teql_sched_data *q = qdisc_priv(sch);

	__skb_queue_head(&q->q, skb);
	sch->qstats.requeues++;
	return 0;
}

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

	skb = __skb_dequeue(&dat->q);
	if (skb == NULL) {
		struct net_device *m = dat->m->dev->qdisc->dev;
		if (m) {
			dat->m->slaves = sch;
			netif_wake_queue(m);
		}
	}
	sch->q.qlen = dat->q.qlen + dat->m->dev->qdisc->q.qlen;
	return skb;
}

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;

	if ((prev = master->slaves) != NULL) {
		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) {
						master->slaves = NULL;
						spin_lock_bh(&master->dev->queue_lock);
						qdisc_reset(master->dev->qdisc);
						spin_unlock_bh(&master->dev->queue_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 rtattr *opt)
{
	struct net_device *dev = sch->dev;
	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;
}

/* "teql*" netdevice routines */

static int
__teql_resolve(struct sk_buff *skb, struct sk_buff *skb_res, struct net_device *dev)
{
	struct teql_sched_data *q = qdisc_priv(dev->qdisc);
	struct neighbour *mn = skb->dst->neighbour;
	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;
		read_lock(&n->lock);
		err = dev->hard_header(skb, dev, ntohs(skb->protocol), n->ha, NULL, skb->len);
		read_unlock(&n->lock);
		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)
{
	if (dev->hard_header == NULL ||
	    skb->dst == NULL ||
	    skb->dst->neighbour == NULL)
		return 0;
	return __teql_resolve(skb, skb_res, dev);
}

static int 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 len = skb->len;
	struct sk_buff *skb_res = NULL;

	start = master->slaves;

restart:
	nores = 0;
	busy = 0;

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

	do {
		struct net_device *slave = q->dev;

		if (slave->qdisc_sleeping != q)
			continue;
		if (netif_queue_stopped(slave) || ! netif_running(slave)) {
			busy = 1;
			continue;
		}

		switch (teql_resolve(skb, skb_res, slave)) {
		case 0:
			if (netif_tx_trylock(slave)) {
				if (!netif_queue_stopped(slave) &&
				    slave->hard_start_xmit(skb, slave) == 0) {
					netif_tx_unlock(slave);
					master->slaves = NEXT_SLAVE(q);
					netif_wake_queue(dev);
					master->stats.tx_packets++;
					master->stats.tx_bytes += len;
					return 0;
				}
				netif_tx_unlock(slave);
			}
			if (netif_queue_stopped(dev))
				busy = 1;
			break;
		case 1:
			master->slaves = NEXT_SLAVE(q);
			return 0;
		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 1;
	}
	master->stats.tx_errors++;

drop:
	master->stats.tx_dropped++;
	dev_kfree_skb(skb);
	return 0;
}

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

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

	flags = FMASK;

	q = m->slaves;
	do {
		struct net_device *slave = q->dev;

		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 net_device_stats *teql_master_stats(struct net_device *dev)
{
	struct teql_master *m = netdev_priv(dev);
	return &m->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 > q->dev->mtu)
				return -EINVAL;
		} while ((q=NEXT_SLAVE(q)) != m->slaves);
	}

	dev->mtu = new_mtu;
	return 0;
}

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->requeue	=	teql_requeue;
	ops->init	=	teql_qdisc_init;
	ops->reset	=	teql_reset;
	ops->destroy	=	teql_destroy;
	ops->owner	=	THIS_MODULE;

	dev->open		= teql_master_open;
	dev->hard_start_xmit	= teql_master_xmit;
	dev->stop		= teql_master_close;
	dev->get_stats		= teql_master_stats;
	dev->change_mtu		= teql_master_mtu;
	dev->type		= ARPHRD_VOID;
	dev->mtu		= 1500;
	dev->tx_queue_len	= 100;
	dev->flags		= IFF_NOARP;
	dev->hard_header_len	= LL_MAX_HEADER;
	SET_MODULE_OWNER(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", 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>
	12	#include <asm/uaccess.h>
	13	#include <asm/system.h>
	14	#include <linux/bitops.h>
	15	#include <linux/types.h>
	16	#include <linux/kernel.h>
1da177e4 LT	17	#include <linux/string.h>
	18	#include <linux/mm.h>
	19	#include <linux/socket.h>
	20	#include <linux/sockios.h>
	21	#include <linux/in.h>
	22	#include <linux/errno.h>
	23	#include <linux/interrupt.h>
14c85021	24	#include <linux/if_arp.h>
1da177e4 LT	25	#include <linux/if_ether.h>
	26	#include <linux/inet.h>
	27	#include <linux/netdevice.h>
	28	#include <linux/etherdevice.h>
	29	#include <linux/notifier.h>
	30	#include <linux/init.h>
	31	#include <net/ip.h>
	32	#include <net/route.h>
	33	#include <linux/skbuff.h>
	34	#include <linux/moduleparam.h>
	35	#include <net/sock.h>
	36	#include <net/pkt_sched.h>
	37
	38	/*
	39	How to setup it.
	40	----------------
	41
	42	After loading this module you will find a new device teqlN
	43	and new qdisc with the same name. To join a slave to the equalizer
	44	you should just set this qdisc on a device f.e.
	45
	46	# tc qdisc add dev eth0 root teql0
	47	# tc qdisc add dev eth1 root teql0
	48
	49	That's all. Full PnP 8)
	50
	51	Applicability.
	52	--------------
	53
	54	1. Slave devices MUST be active devices, i.e., they must raise the tbusy
	55	signal and generate EOI events. If you want to equalize virtual devices
	56	like tunnels, use a normal eql device.
	57	2. This device puts no limitations on physical slave characteristics
	58	f.e. it will equalize 9600baud line and 100Mb ethernet perfectly :-)
	59	Certainly, large difference in link speeds will make the resulting
	60	eqalized link unusable, because of huge packet reordering.
	61	I estimate an upper useful difference as ~10 times.
	62	3. If the slave requires address resolution, only protocols using
	63	neighbour cache (IPv4/IPv6) will work over the equalized link.
	64	Other protocols are still allowed to use the slave device directly,
	65	which will not break load balancing, though native slave
	66	traffic will have the highest priority. */
	67
	68	struct teql_master
	69	{
	70	struct Qdisc_ops qops;
	71	struct net_device *dev;
	72	struct Qdisc *slaves;
	73	struct list_head master_list;
	74	struct net_device_stats stats;
	75	};
	76
	77	struct teql_sched_data
	78	{
	79	struct Qdisc *next;
	80	struct teql_master *m;
	81	struct neighbour *ncache;
	82	struct sk_buff_head q;
	83	};
	84
	85	#define NEXT_SLAVE(q) (((struct teql_sched_data*)qdisc_priv(q))->next)
	86
	87	#define FMASK (IFF_BROADCAST\|IFF_POINTOPOINT\|IFF_BROADCAST)
	88
89	/* "teql" qdisc routines /
90
91	static int
92	teql_enqueue(struct sk_buff skb, struct Qdisc sch)
93	{
94	struct net_device *dev = sch->dev;
95	struct teql_sched_data *q = qdisc_priv(sch);
96
4cd8c9e8 KK	97	if (q->q.qlen < dev->tx_queue_len) {
4cd8c9e8 KK	98	__skb_queue_tail(&q->q, skb);
1da177e4 LT	99	sch->bstats.bytes += skb->len;
	100	sch->bstats.packets++;
	101	return 0;
	102	}
	103
1da177e4 LT	104	kfree_skb(skb);
	105	sch->qstats.drops++;
	106	return NET_XMIT_DROP;
	107	}
	108
	109	static int
	110	teql_requeue(struct sk_buff skb, struct Qdisc sch)
	111	{
	112	struct teql_sched_data *q = qdisc_priv(sch);
	113
	114	__skb_queue_head(&q->q, skb);
	115	sch->qstats.requeues++;
	116	return 0;
	117	}
	118
	119	static struct sk_buff *
	120	teql_dequeue(struct Qdisc* sch)
	121	{
	122	struct teql_sched_data *dat = qdisc_priv(sch);
	123	struct sk_buff *skb;
	124
	125	skb = __skb_dequeue(&dat->q);
	126	if (skb == NULL) {
	127	struct net_device *m = dat->m->dev->qdisc->dev;
	128	if (m) {
	129	dat->m->slaves = sch;
	130	netif_wake_queue(m);
	131	}
	132	}
	133	sch->q.qlen = dat->q.qlen + dat->m->dev->qdisc->q.qlen;
	134	return skb;
	135	}
	136
	137	static __inline__ void
	138	teql_neigh_release(struct neighbour *n)
	139	{
	140	if (n)
	141	neigh_release(n);
	142	}
	143
	144	static void
	145	teql_reset(struct Qdisc* sch)
	146	{
	147	struct teql_sched_data *dat = qdisc_priv(sch);
	148
	149	skb_queue_purge(&dat->q);
	150	sch->q.qlen = 0;
	151	teql_neigh_release(xchg(&dat->ncache, NULL));
	152	}
	153
	154	static void
	155	teql_destroy(struct Qdisc* sch)
	156	{
	157	struct Qdisc q, prev;
	158	struct teql_sched_data *dat = qdisc_priv(sch);
	159	struct teql_master *master = dat->m;
	160
	161	if ((prev = master->slaves) != NULL) {
	162	do {
	163	q = NEXT_SLAVE(prev);
	164	if (q == sch) {
	165	NEXT_SLAVE(prev) = NEXT_SLAVE(q);
	166	if (q == master->slaves) {
	167	master->slaves = NEXT_SLAVE(q);
168	if (q == master->slaves) {
169	master->slaves = NULL;
170	spin_lock_bh(&master->dev->queue_lock);
171	qdisc_reset(master->dev->qdisc);
172	spin_unlock_bh(&master->dev->queue_lock);
173	}
174	}
175	skb_queue_purge(&dat->q);
176	teql_neigh_release(xchg(&dat->ncache, NULL));
177	break;
178	}
10297b99	179
1da177e4 LT	180	} while ((prev = q) != master->slaves);
	181	}
	182	}
	183
	184	static int teql_qdisc_init(struct Qdisc sch, struct rtattr opt)
	185	{
	186	struct net_device *dev = sch->dev;
	187	struct teql_master m = (struct teql_master)sch->ops;
	188	struct teql_sched_data *q = qdisc_priv(sch);
	189
	190	if (dev->hard_header_len > m->dev->hard_header_len)
	191	return -EINVAL;
	192
	193	if (m->dev == dev)
	194	return -ELOOP;
	195
	196	q->m = m;
	197
	198	skb_queue_head_init(&q->q);
	199
	200	if (m->slaves) {
	201	if (m->dev->flags & IFF_UP) {
	202	if ((m->dev->flags&IFF_POINTOPOINT && !(dev->flags&IFF_POINTOPOINT))
	203	\|\| (m->dev->flags&IFF_BROADCAST && !(dev->flags&IFF_BROADCAST))
	204	\|\| (m->dev->flags&IFF_MULTICAST && !(dev->flags&IFF_MULTICAST))
	205	\|\| dev->mtu < m->dev->mtu)
	206	return -EINVAL;
	207	} else {
	208	if (!(dev->flags&IFF_POINTOPOINT))
	209	m->dev->flags &= ~IFF_POINTOPOINT;
	210	if (!(dev->flags&IFF_BROADCAST))
	211	m->dev->flags &= ~IFF_BROADCAST;
	212	if (!(dev->flags&IFF_MULTICAST))
	213	m->dev->flags &= ~IFF_MULTICAST;
	214	if (dev->mtu < m->dev->mtu)
	215	m->dev->mtu = dev->mtu;
	216	}
	217	q->next = NEXT_SLAVE(m->slaves);
	218	NEXT_SLAVE(m->slaves) = sch;
	219	} else {
	220	q->next = sch;
	221	m->slaves = sch;
	222	m->dev->mtu = dev->mtu;
	223	m->dev->flags = (m->dev->flags&~FMASK)\|(dev->flags&FMASK);
	224	}
	225	return 0;
	226	}
	227
	228	/* "teql" netdevice routines /
	229
	230	static int
	231	__teql_resolve(struct sk_buff skb, struct sk_buff skb_res, struct net_device *dev)
	232	{
	233	struct teql_sched_data *q = qdisc_priv(dev->qdisc);
	234	struct neighbour *mn = skb->dst->neighbour;
	235	struct neighbour *n = q->ncache;
	236
	237	if (mn->tbl == NULL)
	238	return -EINVAL;
	239	if (n && n->tbl == mn->tbl &&
	240	memcmp(n->primary_key, mn->primary_key, mn->tbl->key_len) == 0) {
	241	atomic_inc(&n->refcnt);
	242	} else {
	243	n = __neigh_lookup_errno(mn->tbl, mn->primary_key, dev);
244	if (IS_ERR(n))
245	return PTR_ERR(n);
246	}
247	if (neigh_event_send(n, skb_res) == 0) {
248	int err;
249	read_lock(&n->lock);
250	err = dev->hard_header(skb, dev, ntohs(skb->protocol), n->ha, NULL, skb->len);
251	read_unlock(&n->lock);
252	if (err < 0) {
253	neigh_release(n);
254	return -EINVAL;
255	}
256	teql_neigh_release(xchg(&q->ncache, n));
257	return 0;
258	}
259	neigh_release(n);
260	return (skb_res == NULL) ? -EAGAIN : 1;
261	}
262
263	static __inline__ int
264	teql_resolve(struct sk_buff skb, struct sk_buff skb_res, struct net_device *dev)
265	{
266	if (dev->hard_header == NULL \|\|
267	skb->dst == NULL \|\|
268	skb->dst->neighbour == NULL)
269	return 0;
270	return __teql_resolve(skb, skb_res, dev);
271	}
272
273	static int teql_master_xmit(struct sk_buff skb, struct net_device dev)
274	{
2941a486	275	struct teql_master *master = netdev_priv(dev);
1da177e4 LT	276	struct Qdisc start, q;
	277	int busy;
	278	int nores;
	279	int len = skb->len;
	280	struct sk_buff *skb_res = NULL;
	281
	282	start = master->slaves;
	283
	284	restart:
	285	nores = 0;
	286	busy = 0;
	287
	288	if ((q = start) == NULL)
	289	goto drop;
	290
	291	do {
	292	struct net_device *slave = q->dev;
10297b99	293
1da177e4 LT	294	if (slave->qdisc_sleeping != q)
	295	continue;
	296	if (netif_queue_stopped(slave) \|\| ! netif_running(slave)) {
	297	busy = 1;
	298	continue;
	299	}
	300
	301	switch (teql_resolve(skb, skb_res, slave)) {
	302	case 0:
932ff279	303	if (netif_tx_trylock(slave)) {
1da177e4 LT	304	if (!netif_queue_stopped(slave) &&
1da177e4 LT	305	slave->hard_start_xmit(skb, slave) == 0) {
932ff279	306	netif_tx_unlock(slave);
1da177e4 LT	307	master->slaves = NEXT_SLAVE(q);
	308	netif_wake_queue(dev);
	309	master->stats.tx_packets++;
	310	master->stats.tx_bytes += len;
	311	return 0;
	312	}
932ff279	313	netif_tx_unlock(slave);
1da177e4 LT	314	}
	315	if (netif_queue_stopped(dev))
	316	busy = 1;
	317	break;
	318	case 1:
	319	master->slaves = NEXT_SLAVE(q);
	320	return 0;
	321	default:
	322	nores = 1;
	323	break;
	324	}
bbe735e4	325	__skb_pull(skb, skb_network_offset(skb));
1da177e4 LT	326	} while ((q = NEXT_SLAVE(q)) != start);
	327
	328	if (nores && skb_res == NULL) {
	329	skb_res = skb;
	330	goto restart;
	331	}
	332
	333	if (busy) {
	334	netif_stop_queue(dev);
	335	return 1;
	336	}
	337	master->stats.tx_errors++;
	338
	339	drop:
	340	master->stats.tx_dropped++;
	341	dev_kfree_skb(skb);
	342	return 0;
	343	}
	344
	345	static int teql_master_open(struct net_device *dev)
	346	{
	347	struct Qdisc * q;
2941a486	348	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	349	int mtu = 0xFFFE;
	350	unsigned flags = IFF_NOARP\|IFF_MULTICAST;
	351
	352	if (m->slaves == NULL)
	353	return -EUNATCH;
	354
	355	flags = FMASK;
	356
	357	q = m->slaves;
	358	do {
	359	struct net_device *slave = q->dev;
	360
	361	if (slave == NULL)
	362	return -EUNATCH;
	363
	364	if (slave->mtu < mtu)
	365	mtu = slave->mtu;
	366	if (slave->hard_header_len > LL_MAX_HEADER)
	367	return -EINVAL;
	368
	369	/* If all the slaves are BROADCAST, master is BROADCAST
	370	If all the slaves are PtP, master is PtP
	371	Otherwise, master is NBMA.
	372	*/
	373	if (!(slave->flags&IFF_POINTOPOINT))
	374	flags &= ~IFF_POINTOPOINT;
	375	if (!(slave->flags&IFF_BROADCAST))
	376	flags &= ~IFF_BROADCAST;
	377	if (!(slave->flags&IFF_MULTICAST))
	378	flags &= ~IFF_MULTICAST;
	379	} while ((q = NEXT_SLAVE(q)) != m->slaves);
	380
	381	m->dev->mtu = mtu;
	382	m->dev->flags = (m->dev->flags&~FMASK) \| flags;
	383	netif_start_queue(m->dev);
	384	return 0;
	385	}
	386
	387	static int teql_master_close(struct net_device *dev)
	388	{
	389	netif_stop_queue(dev);
	390	return 0;
	391	}
	392
	393	static struct net_device_stats teql_master_stats(struct net_device dev)
	394	{
2941a486	395	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	396	return &m->stats;
	397	}
	398
	399	static int teql_master_mtu(struct net_device *dev, int new_mtu)
	400	{
2941a486	401	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	402	struct Qdisc *q;
	403
	404	if (new_mtu < 68)
	405	return -EINVAL;
	406
	407	q = m->slaves;
	408	if (q) {
	409	do {
	410	if (new_mtu > q->dev->mtu)
	411	return -EINVAL;
	412	} while ((q=NEXT_SLAVE(q)) != m->slaves);
	413	}
	414
	415	dev->mtu = new_mtu;
	416	return 0;
	417	}
	418
	419	static __init void teql_master_setup(struct net_device *dev)
	420	{
2941a486	421	struct teql_master *master = netdev_priv(dev);
1da177e4 LT	422	struct Qdisc_ops *ops = &master->qops;
	423
	424	master->dev = dev;
	425	ops->priv_size = sizeof(struct teql_sched_data);
10297b99	426
1da177e4 LT	427	ops->enqueue = teql_enqueue;
	428	ops->dequeue = teql_dequeue;
	429	ops->requeue = teql_requeue;
	430	ops->init = teql_qdisc_init;
	431	ops->reset = teql_reset;
	432	ops->destroy = teql_destroy;
	433	ops->owner = THIS_MODULE;
	434
	435	dev->open = teql_master_open;
	436	dev->hard_start_xmit = teql_master_xmit;
	437	dev->stop = teql_master_close;
	438	dev->get_stats = teql_master_stats;
	439	dev->change_mtu = teql_master_mtu;
	440	dev->type = ARPHRD_VOID;
	441	dev->mtu = 1500;
	442	dev->tx_queue_len = 100;
	443	dev->flags = IFF_NOARP;
	444	dev->hard_header_len = LL_MAX_HEADER;
	445	SET_MODULE_OWNER(dev);
	446	}
	447
	448	static LIST_HEAD(master_dev_list);
	449	static int max_equalizers = 1;
	450	module_param(max_equalizers, int, 0);
	451	MODULE_PARM_DESC(max_equalizers, "Max number of link equalizers");
	452
	453	static int __init teql_init(void)
	454	{
	455	int i;
	456	int err = -ENODEV;
	457
	458	for (i = 0; i < max_equalizers; i++) {
	459	struct net_device *dev;
	460	struct teql_master *master;
	461
	462	dev = alloc_netdev(sizeof(struct teql_master),
	463	"teql%d", teql_master_setup);
	464	if (!dev) {
	465	err = -ENOMEM;
	466	break;
	467	}
	468
	469	if ((err = register_netdev(dev))) {
	470	free_netdev(dev);
	471	break;
	472	}
	473
2941a486	474	master = netdev_priv(dev);
1da177e4 LT	475
	476	strlcpy(master->qops.id, dev->name, IFNAMSIZ);
	477	err = register_qdisc(&master->qops);
	478
	479	if (err) {
	480	unregister_netdev(dev);
	481	free_netdev(dev);
	482	break;
	483	}
	484
	485	list_add_tail(&master->master_list, &master_dev_list);
	486	}
	487	return i ? 0 : err;
	488	}
	489
10297b99	490	static void __exit teql_exit(void)
1da177e4 LT	491	{
	492	struct teql_master master, nxt;
	493
	494	list_for_each_entry_safe(master, nxt, &master_dev_list, master_list) {
	495
	496	list_del(&master->master_list);
	497
	498	unregister_qdisc(&master->qops);
	499	unregister_netdev(master->dev);
	500	free_netdev(master->dev);
	501	}
	502	}
	503
	504	module_init(teql_init);
	505	module_exit(teql_exit);
	506
	507	MODULE_LICENSE("GPL");