[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;
	int subq = skb->queue_mapping;
	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_subqueue_stopped(slave, subq) ||
		    !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) &&
				    !netif_subqueue_stopped(slave, subq) &&
				    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;
f25f4e44	280	int subq = skb->queue_mapping;
1da177e4 LT	281	struct sk_buff *skb_res = NULL;
	282
	283	start = master->slaves;
	284
	285	restart:
	286	nores = 0;
	287	busy = 0;
	288
	289	if ((q = start) == NULL)
	290	goto drop;
	291
	292	do {
	293	struct net_device *slave = q->dev;
10297b99	294
1da177e4 LT	295	if (slave->qdisc_sleeping != q)
1da177e4 LT	296	continue;
f25f4e44 PWJ	297	if (netif_queue_stopped(slave) \|\|
	298	netif_subqueue_stopped(slave, subq) \|\|
	299	!netif_running(slave)) {
1da177e4 LT	300	busy = 1;
	301	continue;
	302	}
	303
	304	switch (teql_resolve(skb, skb_res, slave)) {
	305	case 0:
932ff279	306	if (netif_tx_trylock(slave)) {
1da177e4	307	if (!netif_queue_stopped(slave) &&
f25f4e44	308	!netif_subqueue_stopped(slave, subq) &&
1da177e4	309	slave->hard_start_xmit(skb, slave) == 0) {
932ff279	310	netif_tx_unlock(slave);
1da177e4 LT	311	master->slaves = NEXT_SLAVE(q);
	312	netif_wake_queue(dev);
	313	master->stats.tx_packets++;
	314	master->stats.tx_bytes += len;
	315	return 0;
	316	}
932ff279	317	netif_tx_unlock(slave);
1da177e4 LT	318	}
	319	if (netif_queue_stopped(dev))
	320	busy = 1;
	321	break;
	322	case 1:
	323	master->slaves = NEXT_SLAVE(q);
	324	return 0;
	325	default:
	326	nores = 1;
	327	break;
	328	}
bbe735e4	329	__skb_pull(skb, skb_network_offset(skb));
1da177e4 LT	330	} while ((q = NEXT_SLAVE(q)) != start);
	331
	332	if (nores && skb_res == NULL) {
	333	skb_res = skb;
	334	goto restart;
	335	}
	336
	337	if (busy) {
	338	netif_stop_queue(dev);
	339	return 1;
	340	}
	341	master->stats.tx_errors++;
	342
	343	drop:
	344	master->stats.tx_dropped++;
	345	dev_kfree_skb(skb);
	346	return 0;
	347	}
	348
	349	static int teql_master_open(struct net_device *dev)
	350	{
	351	struct Qdisc * q;
2941a486	352	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	353	int mtu = 0xFFFE;
	354	unsigned flags = IFF_NOARP\|IFF_MULTICAST;
	355
	356	if (m->slaves == NULL)
	357	return -EUNATCH;
	358
	359	flags = FMASK;
	360
	361	q = m->slaves;
	362	do {
	363	struct net_device *slave = q->dev;
	364
	365	if (slave == NULL)
	366	return -EUNATCH;
	367
	368	if (slave->mtu < mtu)
	369	mtu = slave->mtu;
	370	if (slave->hard_header_len > LL_MAX_HEADER)
	371	return -EINVAL;
	372
	373	/* If all the slaves are BROADCAST, master is BROADCAST
	374	If all the slaves are PtP, master is PtP
	375	Otherwise, master is NBMA.
	376	*/
	377	if (!(slave->flags&IFF_POINTOPOINT))
	378	flags &= ~IFF_POINTOPOINT;
	379	if (!(slave->flags&IFF_BROADCAST))
	380	flags &= ~IFF_BROADCAST;
	381	if (!(slave->flags&IFF_MULTICAST))
	382	flags &= ~IFF_MULTICAST;
	383	} while ((q = NEXT_SLAVE(q)) != m->slaves);
	384
	385	m->dev->mtu = mtu;
	386	m->dev->flags = (m->dev->flags&~FMASK) \| flags;
	387	netif_start_queue(m->dev);
	388	return 0;
	389	}
	390
	391	static int teql_master_close(struct net_device *dev)
	392	{
	393	netif_stop_queue(dev);
	394	return 0;
	395	}
	396
	397	static struct net_device_stats teql_master_stats(struct net_device dev)
	398	{
2941a486	399	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	400	return &m->stats;
	401	}
	402
	403	static int teql_master_mtu(struct net_device *dev, int new_mtu)
	404	{
2941a486	405	struct teql_master *m = netdev_priv(dev);
1da177e4 LT	406	struct Qdisc *q;
	407
	408	if (new_mtu < 68)
	409	return -EINVAL;
	410
	411	q = m->slaves;
	412	if (q) {
	413	do {
	414	if (new_mtu > q->dev->mtu)
	415	return -EINVAL;
	416	} while ((q=NEXT_SLAVE(q)) != m->slaves);
	417	}
	418
	419	dev->mtu = new_mtu;
	420	return 0;
	421	}
	422
	423	static __init void teql_master_setup(struct net_device *dev)
	424	{
2941a486	425	struct teql_master *master = netdev_priv(dev);
1da177e4 LT	426	struct Qdisc_ops *ops = &master->qops;
	427
	428	master->dev = dev;
	429	ops->priv_size = sizeof(struct teql_sched_data);
10297b99	430
1da177e4 LT	431	ops->enqueue = teql_enqueue;
	432	ops->dequeue = teql_dequeue;
	433	ops->requeue = teql_requeue;
	434	ops->init = teql_qdisc_init;
	435	ops->reset = teql_reset;
	436	ops->destroy = teql_destroy;
	437	ops->owner = THIS_MODULE;
	438
	439	dev->open = teql_master_open;
	440	dev->hard_start_xmit = teql_master_xmit;
	441	dev->stop = teql_master_close;
	442	dev->get_stats = teql_master_stats;
	443	dev->change_mtu = teql_master_mtu;
	444	dev->type = ARPHRD_VOID;
	445	dev->mtu = 1500;
	446	dev->tx_queue_len = 100;
	447	dev->flags = IFF_NOARP;
	448	dev->hard_header_len = LL_MAX_HEADER;
	449	SET_MODULE_OWNER(dev);
	450	}
	451
	452	static LIST_HEAD(master_dev_list);
	453	static int max_equalizers = 1;
	454	module_param(max_equalizers, int, 0);
	455	MODULE_PARM_DESC(max_equalizers, "Max number of link equalizers");
	456
	457	static int __init teql_init(void)
	458	{
	459	int i;
	460	int err = -ENODEV;
	461
	462	for (i = 0; i < max_equalizers; i++) {
	463	struct net_device *dev;
	464	struct teql_master *master;
	465
	466	dev = alloc_netdev(sizeof(struct teql_master),
	467	"teql%d", teql_master_setup);
	468	if (!dev) {
	469	err = -ENOMEM;
	470	break;
	471	}
	472
	473	if ((err = register_netdev(dev))) {
	474	free_netdev(dev);
	475	break;
	476	}
	477
2941a486	478	master = netdev_priv(dev);
1da177e4 LT	479
	480	strlcpy(master->qops.id, dev->name, IFNAMSIZ);
	481	err = register_qdisc(&master->qops);
	482
	483	if (err) {
	484	unregister_netdev(dev);
	485	free_netdev(dev);
	486	break;
	487	}
	488
	489	list_add_tail(&master->master_list, &master_dev_list);
	490	}
	491	return i ? 0 : err;
	492	}
	493
10297b99	494	static void __exit teql_exit(void)
1da177e4 LT	495	{
	496	struct teql_master master, nxt;
	497
	498	list_for_each_entry_safe(master, nxt, &master_dev_list, master_list) {
	499
	500	list_del(&master->master_list);
	501
	502	unregister_qdisc(&master->qops);
	503	unregister_netdev(master->dev);
	504	free_netdev(master->dev);
	505	}
	506	}
	507
	508	module_init(teql_init);
	509	module_exit(teql_exit);
	510
	511	MODULE_LICENSE("GPL");