[linux-2.6-block.git] / drivers / net / shaper.c

/*
 *			Simple traffic shaper for Linux NET3.
 *
 *	(c) Copyright 1996 Alan Cox <alan@redhat.com>, All Rights Reserved.
 *				http://www.redhat.com
 *
 *	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.
 *	
 *	Neither Alan Cox nor CymruNet Ltd. admit liability nor provide 
 *	warranty for any of this software. This material is provided 
 *	"AS-IS" and at no charge.	
 *
 *	
 *	Algorithm:
 *
 *	Queue Frame:
 *		Compute time length of frame at regulated speed
 *		Add frame to queue at appropriate point
 *		Adjust time length computation for followup frames
 *		Any frame that falls outside of its boundaries is freed
 *
 *	We work to the following constants
 *
 *		SHAPER_QLEN	Maximum queued frames
 *		SHAPER_LATENCY	Bounding latency on a frame. Leaving this latency
 *				window drops the frame. This stops us queueing 
 *				frames for a long time and confusing a remote
 *				host.
 *		SHAPER_MAXSLIP	Maximum time a priority frame may jump forward.
 *				That bounds the penalty we will inflict on low
 *				priority traffic.
 *		SHAPER_BURST	Time range we call "now" in order to reduce
 *				system load. The more we make this the burstier
 *				the behaviour, the better local performance you
 *				get through packet clustering on routers and the
 *				worse the remote end gets to judge rtts.
 *
 *	This is designed to handle lower speed links ( < 200K/second or so). We
 *	run off a 100-150Hz base clock typically. This gives us a resolution at
 *	200Kbit/second of about 2Kbit or 256 bytes. Above that our timer
 *	resolution may start to cause much more burstiness in the traffic. We
 *	could avoid a lot of that by calling kick_shaper() at the end of the 
 *	tied device transmissions. If you run above about 100K second you 
 *	may need to tune the supposed speed rate for the right values.
 *
 *	BUGS:
 *		Downing the interface under the shaper before the shaper
 *		will render your machine defunct. Don't for now shape over
 *		PPP or SLIP therefore!
 *		This will be fixed in BETA4
 *
 * Update History :
 *
 *              bh_atomic() SMP races fixes and rewritten the locking code to
 *              be SMP safe and irq-mask friendly.
 *              NOTE: we can't use start_bh_atomic() in kick_shaper()
 *              because it's going to be recalled from an irq handler,
 *              and synchronize_bh() is a nono if called from irq context.
 *						1999  Andrea Arcangeli
 *
 *              Device statistics (tx_pakets, tx_bytes,
 *              tx_drops: queue_over_time and collisions: max_queue_exceded)
 *                               1999/06/18 Jordi Murgo <savage@apostols.org>
 *
 *		Use skb->cb for private data.
 *				 2000/03 Andi Kleen
 */
 
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/fcntl.h>
#include <linux/mm.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/skbuff.h>
#include <linux/if_arp.h>
#include <linux/init.h>
#include <linux/if_shaper.h>
#include <linux/jiffies.h>

#include <net/dst.h>
#include <net/arp.h>

struct shaper_cb { 
	unsigned long	shapeclock;		/* Time it should go out */
	unsigned long	shapestamp;		/* Stamp for shaper    */
	__u32		shapelatency;		/* Latency on frame */
	__u32		shapelen;		/* Frame length in clocks */
	__u16		shapepend;		/* Pending */
}; 
#define SHAPERCB(skb) ((struct shaper_cb *) ((skb)->cb))

static int sh_debug;		/* Debug flag */

#define SHAPER_BANNER	"CymruNet Traffic Shaper BETA 0.04 for Linux 2.1\n"

static void shaper_kick(struct shaper *sh);

/*
 *	Compute clocks on a buffer
 */
  
static int shaper_clocks(struct shaper *shaper, struct sk_buff *skb)
{
 	int t=skb->len/shaper->bytespertick;
 	return t;
}

/*
 *	Set the speed of a shaper. We compute this in bytes per tick since
 *	thats how the machine wants to run. Quoted input is in bits per second
 *	as is traditional (note not BAUD). We assume 8 bit bytes. 
 */
  
static void shaper_setspeed(struct shaper *shaper, int bitspersec)
{
	shaper->bitspersec=bitspersec;
	shaper->bytespertick=(bitspersec/HZ)/8;
	if(!shaper->bytespertick)
		shaper->bytespertick++;
}

/*
 *	Throw a frame at a shaper.
 */
  

static int shaper_start_xmit(struct sk_buff *skb, struct net_device *dev)
{
	struct shaper *shaper = dev->priv;
 	struct sk_buff *ptr;
  
	spin_lock(&shaper->lock);
 	ptr=shaper->sendq.prev;
 	
 	/*
 	 *	Set up our packet details
 	 */
 	 
 	SHAPERCB(skb)->shapelatency=0;
 	SHAPERCB(skb)->shapeclock=shaper->recovery;
 	if(time_before(SHAPERCB(skb)->shapeclock, jiffies))
 		SHAPERCB(skb)->shapeclock=jiffies;
 	skb->priority=0;	/* short term bug fix */
 	SHAPERCB(skb)->shapestamp=jiffies;
 	
 	/*
 	 *	Time slots for this packet.
 	 */
 	 
 	SHAPERCB(skb)->shapelen= shaper_clocks(shaper,skb);
 	
	{
		struct sk_buff *tmp;
		/*
		 *	Up our shape clock by the time pending on the queue
		 *	(Should keep this in the shaper as a variable..)
		 */
		for(tmp=skb_peek(&shaper->sendq); tmp!=NULL && 
			tmp!=(struct sk_buff *)&shaper->sendq; tmp=tmp->next)
			SHAPERCB(skb)->shapeclock+=SHAPERCB(tmp)->shapelen;
		/*
		 *	Queue over time. Spill packet.
		 */
		if(time_after(SHAPERCB(skb)->shapeclock,jiffies + SHAPER_LATENCY)) {
			dev_kfree_skb(skb);
			shaper->stats.tx_dropped++;
		} else
			skb_queue_tail(&shaper->sendq, skb);
	}

	if(sh_debug)
 		printk("Frame queued.\n");
 	if(skb_queue_len(&shaper->sendq)>SHAPER_QLEN)
 	{
 		ptr=skb_dequeue(&shaper->sendq);
                dev_kfree_skb(ptr);
                shaper->stats.collisions++;
 	}
	shaper_kick(shaper);
	spin_unlock(&shaper->lock);
 	return 0;
}

/*
 *	Transmit from a shaper
 */
 
static void shaper_queue_xmit(struct shaper *shaper, struct sk_buff *skb)
{
	struct sk_buff *newskb=skb_clone(skb, GFP_ATOMIC);
	if(sh_debug)
		printk("Kick frame on %p\n",newskb);
	if(newskb)
	{
		newskb->dev=shaper->dev;
		newskb->priority=2;
		if(sh_debug)
			printk("Kick new frame to %s, %d\n",
				shaper->dev->name,newskb->priority);
		dev_queue_xmit(newskb);

                shaper->stats.tx_bytes += skb->len;
		shaper->stats.tx_packets++;

                if(sh_debug)
			printk("Kicked new frame out.\n");
		dev_kfree_skb(skb);
	}
}

/*
 *	Timer handler for shaping clock
 */
 
static void shaper_timer(unsigned long data)
{
	struct shaper *shaper = (struct shaper *)data;

	spin_lock(&shaper->lock);
	shaper_kick(shaper);
	spin_unlock(&shaper->lock);
}

/*
 *	Kick a shaper queue and try and do something sensible with the 
 *	queue. 
 */

static void shaper_kick(struct shaper *shaper)
{
	struct sk_buff *skb;
	
	/*
	 *	Walk the list (may be empty)
	 */
	 
	while((skb=skb_peek(&shaper->sendq))!=NULL)
	{
		/*
		 *	Each packet due to go out by now (within an error
		 *	of SHAPER_BURST) gets kicked onto the link 
		 */
		 
		if(sh_debug)
			printk("Clock = %ld, jiffies = %ld\n", SHAPERCB(skb)->shapeclock, jiffies);
		if(time_before_eq(SHAPERCB(skb)->shapeclock, jiffies + SHAPER_BURST))
		{
			/*
			 *	Pull the frame and get interrupts back on.
			 */
			 
			skb_unlink(skb, &shaper->sendq);
			if (shaper->recovery < 
			    SHAPERCB(skb)->shapeclock + SHAPERCB(skb)->shapelen)
				shaper->recovery = SHAPERCB(skb)->shapeclock + SHAPERCB(skb)->shapelen;
			/*
			 *	Pass on to the physical target device via
			 *	our low level packet thrower.
			 */
			
			SHAPERCB(skb)->shapepend=0;
			shaper_queue_xmit(shaper, skb);	/* Fire */
		}
		else
			break;
	}

	/*
	 *	Next kick.
	 */
	 
	if(skb!=NULL)
		mod_timer(&shaper->timer, SHAPERCB(skb)->shapeclock);
}


/*
 *	Bring the interface up. We just disallow this until a 
 *	bind.
 */

static int shaper_open(struct net_device *dev)
{
	struct shaper *shaper=dev->priv;
	
	/*
	 *	Can't open until attached.
	 *	Also can't open until speed is set, or we'll get
	 *	a division by zero.
	 */
	 
	if(shaper->dev==NULL)
		return -ENODEV;
	if(shaper->bitspersec==0)
		return -EINVAL;
	return 0;
}

/*
 *	Closing a shaper flushes the queues.
 */
 
static int shaper_close(struct net_device *dev)
{
	struct shaper *shaper=dev->priv;
	struct sk_buff *skb;

	while ((skb = skb_dequeue(&shaper->sendq)) != NULL)
		dev_kfree_skb(skb);

	spin_lock_bh(&shaper->lock);
	shaper_kick(shaper);
	spin_unlock_bh(&shaper->lock);

	del_timer_sync(&shaper->timer);
	return 0;
}

/*
 *	Revectored calls. We alter the parameters and call the functions
 *	for our attached device. This enables us to bandwidth allocate after
 *	ARP and other resolutions and not before.
 */

static struct net_device_stats *shaper_get_stats(struct net_device *dev)
{
     	struct shaper *sh=dev->priv;
	return &sh->stats;
}

static int shaper_header(struct sk_buff *skb, struct net_device *dev, 
	unsigned short type, void *daddr, void *saddr, unsigned len)
{
	struct shaper *sh=dev->priv;
	int v;
	if(sh_debug)
		printk("Shaper header\n");
	skb->dev=sh->dev;
	v=sh->hard_header(skb,sh->dev,type,daddr,saddr,len);
	skb->dev=dev;
	return v;
}

static int shaper_rebuild_header(struct sk_buff *skb)
{
	struct shaper *sh=skb->dev->priv;
	struct net_device *dev=skb->dev;
	int v;
	if(sh_debug)
		printk("Shaper rebuild header\n");
	skb->dev=sh->dev;
	v=sh->rebuild_header(skb);
	skb->dev=dev;
	return v;
}

#if 0
static int shaper_cache(struct neighbour *neigh, struct hh_cache *hh)
{
	struct shaper *sh=neigh->dev->priv;
	struct net_device *tmp;
	int ret;
	if(sh_debug)
		printk("Shaper header cache bind\n");
	tmp=neigh->dev;
	neigh->dev=sh->dev;
	ret=sh->hard_header_cache(neigh,hh);
	neigh->dev=tmp;
	return ret;
}

static void shaper_cache_update(struct hh_cache *hh, struct net_device *dev,
	unsigned char *haddr)
{
	struct shaper *sh=dev->priv;
	if(sh_debug)
		printk("Shaper cache update\n");
	sh->header_cache_update(hh, sh->dev, haddr);
}
#endif

#ifdef CONFIG_INET

static int shaper_neigh_setup(struct neighbour *n)
{
#ifdef CONFIG_INET
	if (n->nud_state == NUD_NONE) {
		n->ops = &arp_broken_ops;
		n->output = n->ops->output;
	}
#endif	
	return 0;
}

static int shaper_neigh_setup_dev(struct net_device *dev, struct neigh_parms *p)
{
#ifdef CONFIG_INET
	if (p->tbl->family == AF_INET) {
		p->neigh_setup = shaper_neigh_setup;
		p->ucast_probes = 0;
		p->mcast_probes = 0;
	}
#endif	
	return 0;
}

#else /* !(CONFIG_INET) */

static int shaper_neigh_setup_dev(struct net_device *dev, struct neigh_parms *p)
{
	return 0;
}

#endif

static int shaper_attach(struct net_device *shdev, struct shaper *sh, struct net_device *dev)
{
	sh->dev = dev;
	sh->hard_start_xmit=dev->hard_start_xmit;
	sh->get_stats=dev->get_stats;
	if(dev->hard_header)
	{
		sh->hard_header=dev->hard_header;
		shdev->hard_header = shaper_header;
	}
	else
		shdev->hard_header = NULL;
		
	if(dev->rebuild_header)
	{
		sh->rebuild_header	= dev->rebuild_header;
		shdev->rebuild_header	= shaper_rebuild_header;
	}
	else
		shdev->rebuild_header	= NULL;
	
#if 0
	if(dev->hard_header_cache)
	{
		sh->hard_header_cache	= dev->hard_header_cache;
		shdev->hard_header_cache= shaper_cache;
	}
	else
	{
		shdev->hard_header_cache= NULL;
	}
			
	if(dev->header_cache_update)
	{
		sh->header_cache_update	= dev->header_cache_update;
		shdev->header_cache_update = shaper_cache_update;
	}
	else
		shdev->header_cache_update= NULL;
#else
	shdev->header_cache_update = NULL;
	shdev->hard_header_cache = NULL;
#endif
	shdev->neigh_setup = shaper_neigh_setup_dev;
	
	shdev->hard_header_len=dev->hard_header_len;
	shdev->type=dev->type;
	shdev->addr_len=dev->addr_len;
	shdev->mtu=dev->mtu;
	sh->bitspersec=0;
	return 0;
}

static int shaper_ioctl(struct net_device *dev,  struct ifreq *ifr, int cmd)
{
	struct shaperconf *ss= (struct shaperconf *)&ifr->ifr_ifru;
	struct shaper *sh=dev->priv;
	
	if(ss->ss_cmd == SHAPER_SET_DEV || ss->ss_cmd == SHAPER_SET_SPEED)
	{
		if(!capable(CAP_NET_ADMIN))
			return -EPERM;
	}
	
	switch(ss->ss_cmd)
	{
		case SHAPER_SET_DEV:
		{
			struct net_device *them=__dev_get_by_name(ss->ss_name);
			if(them==NULL)
				return -ENODEV;
			if(sh->dev)
				return -EBUSY;
			return shaper_attach(dev,dev->priv, them);
		}
		case SHAPER_GET_DEV:
			if(sh->dev==NULL)
				return -ENODEV;
			strcpy(ss->ss_name, sh->dev->name);
			return 0;
		case SHAPER_SET_SPEED:
			shaper_setspeed(sh,ss->ss_speed);
			return 0;
		case SHAPER_GET_SPEED:
			ss->ss_speed=sh->bitspersec;
			return 0;
		default:
			return -EINVAL;
	}
}

static void shaper_init_priv(struct net_device *dev)
{
	struct shaper *sh = dev->priv;

	skb_queue_head_init(&sh->sendq);
	init_timer(&sh->timer);
	sh->timer.function=shaper_timer;
	sh->timer.data=(unsigned long)sh;
	spin_lock_init(&sh->lock);
}

/*
 *	Add a shaper device to the system
 */
 
static void __init shaper_setup(struct net_device *dev)
{
	/*
	 *	Set up the shaper.
	 */

	SET_MODULE_OWNER(dev);

	shaper_init_priv(dev);

	dev->open		= shaper_open;
	dev->stop		= shaper_close;
	dev->hard_start_xmit 	= shaper_start_xmit;
	dev->get_stats 		= shaper_get_stats;
	dev->set_multicast_list = NULL;
	
	/*
	 *	Intialise the packet queues
	 */
	 
	/*
	 *	Handlers for when we attach to a device.
	 */

	dev->hard_header 	= shaper_header;
	dev->rebuild_header 	= shaper_rebuild_header;
#if 0
	dev->hard_header_cache	= shaper_cache;
	dev->header_cache_update= shaper_cache_update;
#endif
	dev->neigh_setup	= shaper_neigh_setup_dev;
	dev->do_ioctl		= shaper_ioctl;
	dev->hard_header_len	= 0;
	dev->type		= ARPHRD_ETHER;	/* initially */
	dev->set_mac_address	= NULL;
	dev->mtu		= 1500;
	dev->addr_len		= 0;
	dev->tx_queue_len	= 10;
	dev->flags		= 0;
}
 
static int shapers = 1;
#ifdef MODULE

module_param(shapers, int, 0);
MODULE_PARM_DESC(shapers, "Traffic shaper: maximum number of shapers");

#else /* MODULE */

static int __init set_num_shapers(char *str)
{
	shapers = simple_strtol(str, NULL, 0);
	return 1;
}

__setup("shapers=", set_num_shapers);

#endif /* MODULE */

static struct net_device **devs;

static unsigned int shapers_registered = 0;

static int __init shaper_init(void)
{
	int i;
	size_t alloc_size;
	struct net_device *dev;
	char name[IFNAMSIZ];

	if (shapers < 1)
		return -ENODEV;

	alloc_size = sizeof(*dev) * shapers;
	devs = kmalloc(alloc_size, GFP_KERNEL);
	if (!devs)
		return -ENOMEM;
	memset(devs, 0, alloc_size);

	for (i = 0; i < shapers; i++) {

		snprintf(name, IFNAMSIZ, "shaper%d", i);
		dev = alloc_netdev(sizeof(struct shaper), name,
				   shaper_setup);
		if (!dev) 
			break;

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

		devs[i] = dev;
		shapers_registered++;
	}

	if (!shapers_registered) {
		kfree(devs);
		devs = NULL;
	}

	return (shapers_registered ? 0 : -ENODEV);
}

static void __exit shaper_exit (void)
{
	int i;

	for (i = 0; i < shapers_registered; i++) {
		if (devs[i]) {
			unregister_netdev(devs[i]);
			free_netdev(devs[i]);
		}
	}

	kfree(devs);
	devs = NULL;
}

module_init(shaper_init);
module_exit(shaper_exit);
MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	1	/*
	2	* Simple traffic shaper for Linux NET3.
	3	*
	4	* (c) Copyright 1996 Alan Cox <alan@redhat.com>, All Rights Reserved.
	5	* http://www.redhat.com
	6	*
	7	* This program is free software; you can redistribute it and/or
	8	* modify it under the terms of the GNU General Public License
	9	* as published by the Free Software Foundation; either version
	10	* 2 of the License, or (at your option) any later version.
	11	*
	12	* Neither Alan Cox nor CymruNet Ltd. admit liability nor provide
	13	* warranty for any of this software. This material is provided
	14	* "AS-IS" and at no charge.
	15	*
	16	*
	17	* Algorithm:
	18	*
	19	* Queue Frame:
	20	* Compute time length of frame at regulated speed
	21	* Add frame to queue at appropriate point
	22	* Adjust time length computation for followup frames
	23	* Any frame that falls outside of its boundaries is freed
	24	*
	25	* We work to the following constants
	26	*
	27	* SHAPER_QLEN Maximum queued frames
	28	* SHAPER_LATENCY Bounding latency on a frame. Leaving this latency
	29	* window drops the frame. This stops us queueing
	30	* frames for a long time and confusing a remote
	31	* host.
	32	* SHAPER_MAXSLIP Maximum time a priority frame may jump forward.
	33	* That bounds the penalty we will inflict on low
	34	* priority traffic.
	35	* SHAPER_BURST Time range we call "now" in order to reduce
	36	* system load. The more we make this the burstier
	37	* the behaviour, the better local performance you
	38	* get through packet clustering on routers and the
	39	* worse the remote end gets to judge rtts.
	40	*
	41	* This is designed to handle lower speed links ( < 200K/second or so). We
	42	* run off a 100-150Hz base clock typically. This gives us a resolution at
	43	* 200Kbit/second of about 2Kbit or 256 bytes. Above that our timer
	44	* resolution may start to cause much more burstiness in the traffic. We
	45	* could avoid a lot of that by calling kick_shaper() at the end of the
	46	* tied device transmissions. If you run above about 100K second you
	47	* may need to tune the supposed speed rate for the right values.
	48	*
	49	* BUGS:
	50	* Downing the interface under the shaper before the shaper
	51	* will render your machine defunct. Don't for now shape over
	52	* PPP or SLIP therefore!
	53	* This will be fixed in BETA4
	54	*
	55	* Update History :
	56	*
	57	* bh_atomic() SMP races fixes and rewritten the locking code to
	58	* be SMP safe and irq-mask friendly.
	59	* NOTE: we can't use start_bh_atomic() in kick_shaper()
	60	* because it's going to be recalled from an irq handler,
	61	* and synchronize_bh() is a nono if called from irq context.
	62	* 1999 Andrea Arcangeli
	63	*
	64	* Device statistics (tx_pakets, tx_bytes,
65	* tx_drops: queue_over_time and collisions: max_queue_exceded)
66	* 1999/06/18 Jordi Murgo <savage@apostols.org>
67	*
68	* Use skb->cb for private data.
69	* 2000/03 Andi Kleen
70	*/
71
1da177e4 LT	72	#include <linux/module.h>
	73	#include <linux/kernel.h>
	74	#include <linux/fcntl.h>
	75	#include <linux/mm.h>
	76	#include <linux/slab.h>
	77	#include <linux/string.h>
	78	#include <linux/errno.h>
	79	#include <linux/netdevice.h>
	80	#include <linux/etherdevice.h>
	81	#include <linux/skbuff.h>
	82	#include <linux/if_arp.h>
	83	#include <linux/init.h>
	84	#include <linux/if_shaper.h>
ff5688ae	85	#include <linux/jiffies.h>
1da177e4 LT	86
	87	#include <net/dst.h>
	88	#include <net/arp.h>
	89
	90	struct shaper_cb {
	91	unsigned long shapeclock; /* Time it should go out */
	92	unsigned long shapestamp; /* Stamp for shaper */
	93	__u32 shapelatency; /* Latency on frame */
	94	__u32 shapelen; /* Frame length in clocks */
	95	__u16 shapepend; /* Pending */
	96	};
	97	#define SHAPERCB(skb) ((struct shaper_cb *) ((skb)->cb))
	98
	99	static int sh_debug; /* Debug flag */
	100
	101	#define SHAPER_BANNER "CymruNet Traffic Shaper BETA 0.04 for Linux 2.1\n"
	102
1da177e4 LT	103	static void shaper_kick(struct shaper *sh);
1da177e4 LT	104
1da177e4 LT	105	/*
	106	* Compute clocks on a buffer
	107	*/
	108
	109	static int shaper_clocks(struct shaper shaper, struct sk_buff skb)
	110	{
	111	int t=skb->len/shaper->bytespertick;
	112	return t;
	113	}
	114
	115	/*
	116	* Set the speed of a shaper. We compute this in bytes per tick since
	117	* thats how the machine wants to run. Quoted input is in bits per second
	118	* as is traditional (note not BAUD). We assume 8 bit bytes.
	119	*/
	120
	121	static void shaper_setspeed(struct shaper *shaper, int bitspersec)
	122	{
	123	shaper->bitspersec=bitspersec;
	124	shaper->bytespertick=(bitspersec/HZ)/8;
	125	if(!shaper->bytespertick)
	126	shaper->bytespertick++;
	127	}
	128
	129	/*
	130	* Throw a frame at a shaper.
	131	*/
	132
b597ef47 CH	133
b597ef47 CH	134	static int shaper_start_xmit(struct sk_buff skb, struct net_device dev)
1da177e4	135	{
b597ef47	136	struct shaper *shaper = dev->priv;
1da177e4	137	struct sk_buff *ptr;
bc971dee CH	138
bc971dee CH	139	spin_lock(&shaper->lock);
1da177e4 LT	140	ptr=shaper->sendq.prev;
	141
	142	/*
	143	* Set up our packet details
	144	*/
	145
	146	SHAPERCB(skb)->shapelatency=0;
	147	SHAPERCB(skb)->shapeclock=shaper->recovery;
	148	if(time_before(SHAPERCB(skb)->shapeclock, jiffies))
	149	SHAPERCB(skb)->shapeclock=jiffies;
	150	skb->priority=0; /* short term bug fix */
	151	SHAPERCB(skb)->shapestamp=jiffies;
	152
	153	/*
	154	* Time slots for this packet.
	155	*/
	156
	157	SHAPERCB(skb)->shapelen= shaper_clocks(shaper,skb);
	158
1da177e4 LT	159	{
	160	struct sk_buff *tmp;
	161	/*
	162	* Up our shape clock by the time pending on the queue
	163	* (Should keep this in the shaper as a variable..)
	164	*/
	165	for(tmp=skb_peek(&shaper->sendq); tmp!=NULL &&
	166	tmp!=(struct sk_buff *)&shaper->sendq; tmp=tmp->next)
	167	SHAPERCB(skb)->shapeclock+=SHAPERCB(tmp)->shapelen;
	168	/*
	169	* Queue over time. Spill packet.
	170	*/
ff5688ae	171	if(time_after(SHAPERCB(skb)->shapeclock,jiffies + SHAPER_LATENCY)) {
1da177e4 LT	172	dev_kfree_skb(skb);
	173	shaper->stats.tx_dropped++;
	174	} else
	175	skb_queue_tail(&shaper->sendq, skb);
	176	}
8728b834	177
1da177e4 LT	178	if(sh_debug)
	179	printk("Frame queued.\n");
	180	if(skb_queue_len(&shaper->sendq)>SHAPER_QLEN)
	181	{
	182	ptr=skb_dequeue(&shaper->sendq);
	183	dev_kfree_skb(ptr);
	184	shaper->stats.collisions++;
	185	}
b597ef47	186	shaper_kick(shaper);
bc971dee	187	spin_unlock(&shaper->lock);
1da177e4 LT	188	return 0;
	189	}
	190
	191	/*
	192	* Transmit from a shaper
	193	*/
	194
	195	static void shaper_queue_xmit(struct shaper shaper, struct sk_buff skb)
	196	{
	197	struct sk_buff *newskb=skb_clone(skb, GFP_ATOMIC);
	198	if(sh_debug)
	199	printk("Kick frame on %p\n",newskb);
	200	if(newskb)
	201	{
	202	newskb->dev=shaper->dev;
	203	newskb->priority=2;
	204	if(sh_debug)
	205	printk("Kick new frame to %s, %d\n",
	206	shaper->dev->name,newskb->priority);
	207	dev_queue_xmit(newskb);
	208
	209	shaper->stats.tx_bytes += skb->len;
	210	shaper->stats.tx_packets++;
	211
	212	if(sh_debug)
	213	printk("Kicked new frame out.\n");
	214	dev_kfree_skb(skb);
	215	}
	216	}
	217
	218	/*
	219	* Timer handler for shaping clock
	220	*/
	221
	222	static void shaper_timer(unsigned long data)
	223	{
b597ef47 CH	224	struct shaper shaper = (struct shaper )data;
b597ef47 CH	225
bc971dee CH	226	spin_lock(&shaper->lock);
	227	shaper_kick(shaper);
	228	spin_unlock(&shaper->lock);
1da177e4 LT	229	}
	230
	231	/*
	232	* Kick a shaper queue and try and do something sensible with the
	233	* queue.
	234	*/
	235
	236	static void shaper_kick(struct shaper *shaper)
	237	{
	238	struct sk_buff *skb;
	239
1da177e4 LT	240	/*
	241	* Walk the list (may be empty)
	242	*/
	243
	244	while((skb=skb_peek(&shaper->sendq))!=NULL)
	245	{
	246	/*
	247	* Each packet due to go out by now (within an error
	248	* of SHAPER_BURST) gets kicked onto the link
	249	*/
	250
	251	if(sh_debug)
	252	printk("Clock = %ld, jiffies = %ld\n", SHAPERCB(skb)->shapeclock, jiffies);
	253	if(time_before_eq(SHAPERCB(skb)->shapeclock, jiffies + SHAPER_BURST))
	254	{
	255	/*
	256	* Pull the frame and get interrupts back on.
	257	*/
	258
8728b834	259	skb_unlink(skb, &shaper->sendq);
1da177e4 LT	260	if (shaper->recovery <
	261	SHAPERCB(skb)->shapeclock + SHAPERCB(skb)->shapelen)
	262	shaper->recovery = SHAPERCB(skb)->shapeclock + SHAPERCB(skb)->shapelen;
	263	/*
	264	* Pass on to the physical target device via
	265	* our low level packet thrower.
	266	*/
	267
	268	SHAPERCB(skb)->shapepend=0;
	269	shaper_queue_xmit(shaper, skb); /* Fire */
	270	}
	271	else
	272	break;
	273	}
	274
	275	/*
	276	* Next kick.
	277	*/
	278
	279	if(skb!=NULL)
	280	mod_timer(&shaper->timer, SHAPERCB(skb)->shapeclock);
1da177e4 LT	281	}
	282
	283
1da177e4 LT	284	/*
	285	* Bring the interface up. We just disallow this until a
	286	* bind.
	287	*/
	288
	289	static int shaper_open(struct net_device *dev)
	290	{
	291	struct shaper *shaper=dev->priv;
	292
	293	/*
	294	* Can't open until attached.
	295	* Also can't open until speed is set, or we'll get
	296	* a division by zero.
	297	*/
	298
	299	if(shaper->dev==NULL)
	300	return -ENODEV;
	301	if(shaper->bitspersec==0)
	302	return -EINVAL;
	303	return 0;
	304	}
	305
	306	/*
	307	* Closing a shaper flushes the queues.
	308	*/
	309
	310	static int shaper_close(struct net_device *dev)
	311	{
	312	struct shaper *shaper=dev->priv;
bc971dee CH	313	struct sk_buff *skb;
	314
	315	while ((skb = skb_dequeue(&shaper->sendq)) != NULL)
	316	dev_kfree_skb(skb);
	317
	318	spin_lock_bh(&shaper->lock);
	319	shaper_kick(shaper);
	320	spin_unlock_bh(&shaper->lock);
	321
1da177e4 LT	322	del_timer_sync(&shaper->timer);
	323	return 0;
	324	}
	325
	326	/*
	327	* Revectored calls. We alter the parameters and call the functions
	328	* for our attached device. This enables us to bandwidth allocate after
	329	* ARP and other resolutions and not before.
	330	*/
	331
1da177e4 LT	332	static struct net_device_stats shaper_get_stats(struct net_device dev)
	333	{
	334	struct shaper *sh=dev->priv;
	335	return &sh->stats;
	336	}
	337
	338	static int shaper_header(struct sk_buff skb, struct net_device dev,
	339	unsigned short type, void daddr, void saddr, unsigned len)
	340	{
	341	struct shaper *sh=dev->priv;
	342	int v;
	343	if(sh_debug)
	344	printk("Shaper header\n");
	345	skb->dev=sh->dev;
	346	v=sh->hard_header(skb,sh->dev,type,daddr,saddr,len);
	347	skb->dev=dev;
	348	return v;
	349	}
	350
	351	static int shaper_rebuild_header(struct sk_buff *skb)
	352	{
	353	struct shaper *sh=skb->dev->priv;
	354	struct net_device *dev=skb->dev;
	355	int v;
	356	if(sh_debug)
	357	printk("Shaper rebuild header\n");
	358	skb->dev=sh->dev;
	359	v=sh->rebuild_header(skb);
	360	skb->dev=dev;
	361	return v;
	362	}
	363
	364	#if 0
	365	static int shaper_cache(struct neighbour neigh, struct hh_cache hh)
	366	{
	367	struct shaper *sh=neigh->dev->priv;
	368	struct net_device *tmp;
	369	int ret;
	370	if(sh_debug)
	371	printk("Shaper header cache bind\n");
	372	tmp=neigh->dev;
	373	neigh->dev=sh->dev;
	374	ret=sh->hard_header_cache(neigh,hh);
	375	neigh->dev=tmp;
	376	return ret;
	377	}
	378
	379	static void shaper_cache_update(struct hh_cache hh, struct net_device dev,
	380	unsigned char *haddr)
	381	{
	382	struct shaper *sh=dev->priv;
	383	if(sh_debug)
	384	printk("Shaper cache update\n");
	385	sh->header_cache_update(hh, sh->dev, haddr);
	386	}
	387	#endif
	388
	389	#ifdef CONFIG_INET
	390
	391	static int shaper_neigh_setup(struct neighbour *n)
	392	{
	393	#ifdef CONFIG_INET
	394	if (n->nud_state == NUD_NONE) {
	395	n->ops = &arp_broken_ops;
396	n->output = n->ops->output;
397	}
398	#endif
399	return 0;
400	}
401
402	static int shaper_neigh_setup_dev(struct net_device dev, struct neigh_parms p)
403	{
404	#ifdef CONFIG_INET
405	if (p->tbl->family == AF_INET) {
406	p->neigh_setup = shaper_neigh_setup;
407	p->ucast_probes = 0;
408	p->mcast_probes = 0;
409	}
410	#endif
411	return 0;
412	}
413
414	#else /* !(CONFIG_INET) */
415
416	static int shaper_neigh_setup_dev(struct net_device dev, struct neigh_parms p)
417	{
418	return 0;
419	}
420
421	#endif
422
423	static int shaper_attach(struct net_device shdev, struct shaper sh, struct net_device *dev)
424	{
425	sh->dev = dev;
426	sh->hard_start_xmit=dev->hard_start_xmit;
427	sh->get_stats=dev->get_stats;
428	if(dev->hard_header)
429	{
430	sh->hard_header=dev->hard_header;
431	shdev->hard_header = shaper_header;
432	}
433	else
434	shdev->hard_header = NULL;
435
436	if(dev->rebuild_header)
437	{
438	sh->rebuild_header = dev->rebuild_header;
439	shdev->rebuild_header = shaper_rebuild_header;
440	}
441	else
442	shdev->rebuild_header = NULL;
443
444	#if 0
445	if(dev->hard_header_cache)
446	{
447	sh->hard_header_cache = dev->hard_header_cache;
448	shdev->hard_header_cache= shaper_cache;
449	}
450	else
451	{
452	shdev->hard_header_cache= NULL;
453	}
454
455	if(dev->header_cache_update)
456	{
457	sh->header_cache_update = dev->header_cache_update;
458	shdev->header_cache_update = shaper_cache_update;
459	}
460	else
461	shdev->header_cache_update= NULL;
462	#else
463	shdev->header_cache_update = NULL;
464	shdev->hard_header_cache = NULL;
465	#endif
466	shdev->neigh_setup = shaper_neigh_setup_dev;
467
468	shdev->hard_header_len=dev->hard_header_len;
469	shdev->type=dev->type;
470	shdev->addr_len=dev->addr_len;
471	shdev->mtu=dev->mtu;
472	sh->bitspersec=0;
473	return 0;
474	}
475
476	static int shaper_ioctl(struct net_device dev, struct ifreq ifr, int cmd)
477	{
478	struct shaperconf ss= (struct shaperconf )&ifr->ifr_ifru;
479	struct shaper *sh=dev->priv;
480
481	if(ss->ss_cmd == SHAPER_SET_DEV \|\| ss->ss_cmd == SHAPER_SET_SPEED)
482	{
483	if(!capable(CAP_NET_ADMIN))
484	return -EPERM;
485	}
486
487	switch(ss->ss_cmd)
488	{
489	case SHAPER_SET_DEV:
490	{
491	struct net_device *them=__dev_get_by_name(ss->ss_name);
492	if(them==NULL)
493	return -ENODEV;
494	if(sh->dev)
495	return -EBUSY;
496	return shaper_attach(dev,dev->priv, them);
497	}
498	case SHAPER_GET_DEV:
499	if(sh->dev==NULL)
500	return -ENODEV;
501	strcpy(ss->ss_name, sh->dev->name);
502	return 0;
503	case SHAPER_SET_SPEED:
504	shaper_setspeed(sh,ss->ss_speed);
505	return 0;
506	case SHAPER_GET_SPEED:
507	ss->ss_speed=sh->bitspersec;
508	return 0;
509	default:
510	return -EINVAL;
511	}
512	}
513
514	static void shaper_init_priv(struct net_device *dev)
515	{
516	struct shaper *sh = dev->priv;
517
518	skb_queue_head_init(&sh->sendq);
519	init_timer(&sh->timer);
520	sh->timer.function=shaper_timer;
521	sh->timer.data=(unsigned long)sh;
bc971dee	522	spin_lock_init(&sh->lock);
1da177e4 LT	523	}
	524
	525	/*
	526	* Add a shaper device to the system
	527	*/
	528
	529	static void __init shaper_setup(struct net_device *dev)
	530	{
	531	/*
	532	* Set up the shaper.
	533	*/
	534
	535	SET_MODULE_OWNER(dev);
	536
	537	shaper_init_priv(dev);
	538
	539	dev->open = shaper_open;
	540	dev->stop = shaper_close;
	541	dev->hard_start_xmit = shaper_start_xmit;
	542	dev->get_stats = shaper_get_stats;
	543	dev->set_multicast_list = NULL;
	544
	545	/*
	546	* Intialise the packet queues
	547	*/
	548
	549	/*
	550	* Handlers for when we attach to a device.
	551	*/
	552
	553	dev->hard_header = shaper_header;
	554	dev->rebuild_header = shaper_rebuild_header;
	555	#if 0
	556	dev->hard_header_cache = shaper_cache;
	557	dev->header_cache_update= shaper_cache_update;
	558	#endif
	559	dev->neigh_setup = shaper_neigh_setup_dev;
	560	dev->do_ioctl = shaper_ioctl;
	561	dev->hard_header_len = 0;
	562	dev->type = ARPHRD_ETHER; /* initially */
	563	dev->set_mac_address = NULL;
	564	dev->mtu = 1500;
	565	dev->addr_len = 0;
	566	dev->tx_queue_len = 10;
	567	dev->flags = 0;
	568	}
	569
	570	static int shapers = 1;
	571	#ifdef MODULE
	572
	573	module_param(shapers, int, 0);
	574	MODULE_PARM_DESC(shapers, "Traffic shaper: maximum number of shapers");
	575
	576	#else /* MODULE */
	577
	578	static int __init set_num_shapers(char *str)
	579	{
	580	shapers = simple_strtol(str, NULL, 0);
	581	return 1;
	582	}
	583
	584	__setup("shapers=", set_num_shapers);
	585
	586	#endif /* MODULE */
587
588	static struct net_device **devs;
589
590	static unsigned int shapers_registered = 0;
591
592	static int __init shaper_init(void)
593	{
594	int i;
595	size_t alloc_size;
596	struct net_device *dev;
597	char name[IFNAMSIZ];
598
599	if (shapers < 1)
600	return -ENODEV;
601
602	alloc_size = sizeof(dev) shapers;
603	devs = kmalloc(alloc_size, GFP_KERNEL);
604	if (!devs)
605	return -ENOMEM;
606	memset(devs, 0, alloc_size);
607
608	for (i = 0; i < shapers; i++) {
609
610	snprintf(name, IFNAMSIZ, "shaper%d", i);
611	dev = alloc_netdev(sizeof(struct shaper), name,
612	shaper_setup);
613	if (!dev)
614	break;
615
616	if (register_netdev(dev)) {
617	free_netdev(dev);
618	break;
619	}
620
621	devs[i] = dev;
622	shapers_registered++;
623	}
624
625	if (!shapers_registered) {
626	kfree(devs);
627	devs = NULL;
628	}
629
630	return (shapers_registered ? 0 : -ENODEV);
631	}
632
633	static void __exit shaper_exit (void)
634	{
635	int i;
636
637	for (i = 0; i < shapers_registered; i++) {
638	if (devs[i]) {
639	unregister_netdev(devs[i]);
640	free_netdev(devs[i]);
641	}
642	}
643
644	kfree(devs);
645	devs = NULL;
646	}
647
648	module_init(shaper_init);
649	module_exit(shaper_exit);
650	MODULE_LICENSE("GPL");
651