net/sched/wrr.c

/*
 *  Copyright (c) 2013 Qualcomm Atheros, Inc.
 *
 *  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.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  Weighted round-robin algorithm.
 * 
 *	The qdisc contains number of queues and every of them
 *	is assigned weight. The available bandwidth is distributed
 *	among classes proportionaly to the weights.
 * 
 *  Input Parameters:
 *		1) Number of queues. The default is 4.
 * 		2) fmark: it is assigned to each class for indicating a flow.
 *
 *  It assumes that the filtering data will be send to the root of WRR.
 *
 *
 *                         1:0 (htb)
 *                          |
 *                         1:2 (htb)
 *                          |		                        
 *                         2:0 (wrr)
 *                     /    |   |   \
 *                   2:11 2:12 2:13 2:14
 *
 *	Author:		Tos Xu   tosx@wicresoft.com
 *	Date: 		2008/12/10
 */

#include <linux/module.h>
#include <asm/uaccess.h>
#include <asm/system.h>
#include <asm/bitops.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/sched.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_ether.h>
#include <linux/inet.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/notifier.h>
#include <net/ip.h>
#include <net/route.h>
#include <linux/skbuff.h>
#include <net/sock.h>
#include <net/pkt_sched.h>

#define TOTALQUEUES	4

enum
{
	TCA_WRR_UNSPEC,
	TCA_WRR_INIT,
	TCA_WRR_PARAMS,
	__TCA_WRR_MAX,
};

#define TCA_WRR_MAX (__TCA_WRR_MAX-1)
#define WRR_DEBUG 0

struct wrr_class
{
	u32			classid;
	int			quantum;	/* Max bytes transmited at once */
	u32			nfmark;
	struct Qdisc		*q;		/* Elementary queueing discipline */

	struct tc_stats		stats;
	int			deficit;
	int			refcnt;

    	struct gnet_stats_basic bstats;
    	struct gnet_stats_queue qstats;
    	struct gnet_stats_rate_est rate_est;
};



struct wrr_sched_data
{
	int key;
	int total_classes;
	int total_queues;			
	struct wrr_class * classes[TOTALQUEUES];
	int filters;
};

struct wrr_gopt 
{
	int total_queues;
};

struct wrr_class_opt
{
	int quantum;
	int handle;
};


static __inline__ struct wrr_class *
wrr_find(u32 handle, struct Qdisc *sch)
{
    int i = 0;
    struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
    struct wrr_class ** cl = q->classes;

    for(i=0;i< TOTALQUEUES;i++)
    {
	if(cl[i]==NULL) continue;
	if(cl[i]->classid==handle) return cl[i];
    }

    return NULL;

}

static struct wrr_class *
wrr_clasify(struct sk_buff *skb, struct Qdisc *sch)
{
    struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);

    struct wrr_class **cl = q->classes;

    int no;

    no = skb->mark % TOTALQUEUES;	
	
    if(cl[no]==NULL) return NULL;

    if(cl[no]->nfmark==skb->mark)
	return cl[no];
    else
	return NULL;
}

static int
wrr_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct wrr_class *cl = wrr_clasify(skb,sch);

#if WRR_DEBUG
	printk("wrr_enqueue nfmark=%d.\n",cl->nfmark);	    
#endif
	if (!cl) {
		kfree_skb(skb);
		sch->qstats.drops++;
		return NET_XMIT_DROP;
	}
	
	if (cl->q->enqueue(skb, cl->q) != NET_XMIT_SUCCESS) {
		sch->qstats.drops++;
		cl->stats.drops++;
		return NET_XMIT_DROP;
	}

	sch->q.qlen++;
	sch->bstats.packets++;
	sch->bstats.bytes+=skb->len;
	cl->stats.packets++;
	cl->stats.bytes+=skb->len;

	return NET_XMIT_SUCCESS;
}
#if 0
static int
wrr_requeue(struct sk_buff *skb, struct Qdisc *sch)
{
	struct wrr_class *cl = wrr_clasify(skb,sch);

#if WRR_DEBUG
	printk("wrr_renqueue mark=%d.\n",cl->nfmark);	    
#endif

	if (!cl) {
		kfree_skb(skb);
		sch->qstats.drops++;
		return NET_XMIT_DROP;
	}
	
	if (cl->q->ops->requeue(skb, cl->q) != NET_XMIT_SUCCESS) {
		sch->qstats.drops++;
		cl->stats.drops++;
		return NET_XMIT_DROP;
	}

	sch->q.qlen++;

	return NET_XMIT_SUCCESS;
}
#endif
static struct sk_buff *
wrr_dequeue(struct Qdisc *sch)
{
	struct sk_buff *skb;
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class **cl = q->classes;
	int i,j;
	int done;

	j = q->key;

	do
	{	
		done = 1;

		for(i=0;i<TOTALQUEUES;i++)
		{
			if(cl[j]==NULL)
			{
				j = (j+1)% TOTALQUEUES;
				continue;
			}

			if(cl[j]->deficit <= 0) 
			{
				if(cl[j]->q->q.qlen) done = 0;
				cl[j]->deficit += cl[j]->quantum;
				j = (j+1)% TOTALQUEUES;
				continue;	
			}		
	
			if ((skb = cl[j]->q->dequeue(cl[j]->q)) == NULL)
			{
				j = (j+1)% TOTALQUEUES; 
				continue;
			}
			cl[j]->deficit -= skb->len;

			if (cl[j]->deficit <= 0) 
			{
				j = (j+1)% TOTALQUEUES;			
			}
	
			q->key = j;
			sch->q.qlen--;
#if WRR_DEBUG
			printk("wrr_dequeue mark=%d.\n",skb->mark);
#endif
			return skb;
		}

	}while(!done);

	q->key = 0;
	return NULL;
}

static unsigned int 
wrr_drop(struct Qdisc* sch)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class **cl = q->classes;
	int i,len;	
	
    	for(i=0;i< TOTALQUEUES;i++)
    	{
		if(cl[i]==NULL) continue;
		if (cl[i]->q->ops->drop && (len = cl[i]->q->ops->drop(cl[i]->q)))
			return len;
    	}

	return 0;

}

static void
wrr_reset(struct Qdisc* sch)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class **cl = q->classes;
	int i;

	for(i=0;i< TOTALQUEUES;i++)
	{
		if(cl[i]==NULL) continue;

		qdisc_reset(cl[i]->q);
		cl[i]->deficit = cl[i]->quantum;
	}
	sch->q.qlen = 0;
}

static int 
wrr_init(struct Qdisc *sch, struct nlattr *opt)
{
	struct wrr_sched_data *q = (struct wrr_sched_data*)qdisc_priv(sch);
	struct wrr_gopt * wrr_gopt;
	struct nlattr *tb[TCA_WRR_MAX];
    int err;

	memset(q, 0, sizeof(*q));
	
	q->total_queues = TOTALQUEUES;

	if(opt==NULL)
		goto SETDEFAULT;
    
    err = nla_parse_nested(tb, TCA_WRR_MAX, opt, NULL);
    if (err < 0)
        return err;


	if(tb[TCA_WRR_INIT])
	{
		wrr_gopt = nla_data(tb[TCA_WRR_INIT]);
		if(wrr_gopt==NULL) {
			printk("Total of default queues=%x.\n",TOTALQUEUES);
		}else
			q->total_queues = wrr_gopt->total_queues;

	}	

SETDEFAULT:
	
	if(q->total_queues <0 ||q->total_queues >8)
		return -1;
#if WRR_DEBUG
    	printk("wrr_init sch:%p, handle:%X,total queues:%x.\n",sch,sch->handle,q->total_queues);
#endif

	return 0;
}

static int 
wrr_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct wrr_sched_data *q = (struct wrr_sched_data*)qdisc_priv(sch);
	unsigned char	 *b = skb->tail;
	struct wrr_gopt gopt;
	struct nlattr *rta;

#if WRR_DEBUG
    	printk("wrr_dump sch=%p, handle=%X\n",sch,sch->handle);
#endif
	gopt.total_queues = q->total_queues;

	rta = (struct nlattr *)b;
	RTA_PUT(skb, TCA_OPTIONS, 0, NULL);
	RTA_PUT(skb, TCA_WRR_INIT, sizeof(gopt), &gopt);
	rta->nla_len = skb->tail-b;

	return skb->len;
rtattr_failure:
	skb_trim(skb, b - skb->data);
	return -1;
}

static int
wrr_dump_class(struct Qdisc *sch, unsigned long arg,
	       struct sk_buff *skb, struct tcmsg *tcm)
{
	struct wrr_class *cl = (struct wrr_class*)arg;
	unsigned char	 *b = skb->tail;
	struct wrr_class_opt copt;
	struct nlattr *rta;

#if WRR_DEBUG
    	printk("wrr_dclass sch=%p, handle=%x\n",sch,sch->handle);
#endif
	if(cl==NULL|| tcm==NULL) return -1;

	tcm->tcm_parent = TC_H_ROOT;
	tcm->tcm_handle = cl->classid;
	tcm->tcm_info = cl->q->handle;

	copt.handle = cl->nfmark;
	copt.quantum = cl->quantum;

	rta = (struct nlattr *)b;
	RTA_PUT(skb, TCA_OPTIONS, 0, NULL);
	RTA_PUT(skb, TCA_WRR_PARAMS, sizeof(copt), &copt);
	rta->nla_len = skb->tail-b;

	return skb->len;
rtattr_failure:
	skb_trim(skb, b - skb->data);
	return -1;
}

static int 
wrr_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
		     struct Qdisc **old)
{
	struct wrr_class *cl = (struct wrr_class*)arg;

	if (cl) {
		if (new == NULL) {
			if ((new = qdisc_create_dflt(qdisc_dev(sch),sch->dev_queue, &pfifo_qdisc_ops, cl->classid)) == NULL)
				return -ENOBUFS;
		} 
		if ((*old = xchg(&cl->q, new)) != NULL)
			qdisc_reset(*old);
		return 0;
	}
	return -ENOENT;
}

static struct Qdisc *
wrr_leaf(struct Qdisc *sch, unsigned long arg)
{
	struct wrr_class *cl = (struct wrr_class*)arg;

	return cl ? cl->q : NULL;
}

static unsigned long 
wrr_get(struct Qdisc *sch, u32 classid)
{
 	struct wrr_class *cl = wrr_find(classid,sch);
 	if (cl) cl->refcnt++;
	return (unsigned long)cl;
}

static void 
wrr_destroy_filters(struct wrr_sched_data *q)
{

}

static void 
wrr_destroy_class(struct wrr_class *cl)
{
	qdisc_destroy(cl->q);
	kfree(cl);
}

static void
wrr_destroy(struct Qdisc* sch)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
 	struct wrr_class **cl = q->classes;
	struct wrr_class * p;
	int i;

#if WRR_DEBUG
	printk("wrr_destroy.\n");
#endif
	for(i=0;i< TOTALQUEUES;i++)
	{
		if(cl[i]==NULL) continue;
		p = cl[i];
		cl[i]=NULL;
		wrr_destroy_class(p);
		q->total_classes --;
	}

#if WRR_DEBUG
	printk("wrr_destroy total queues %u.\n",i);
#endif	
	wrr_destroy_filters(q);
	sch->q.qlen=0;
	q->key = 0;

}

static void 
wrr_put(struct Qdisc *sch, unsigned long arg)
{
	struct wrr_class *cl = (struct wrr_class*)arg;


	if (--cl->refcnt == 0) {
		wrr_destroy_class(cl);
	}

}

static int
wrr_change_class(struct Qdisc *sch, u32 classid, u32 parentid, struct nlattr **tca,
		 unsigned long *arg)
{
	int err;
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class *cl = (struct wrr_class*)*arg;
	struct nlattr *opt = tca[TCA_OPTIONS];
	struct nlattr *tb[TCA_WRR_MAX];
	struct wrr_class_opt * wrr_copt;
	int no = 0;

	if(opt==NULL)
		return -EINVAL;

    err = nla_parse_nested(tb, TCA_WRR_MAX, opt, NULL);
    if (err < 0)
        return err;

	if(tb[TCA_WRR_PARAMS])
	{

		wrr_copt = nla_data(tb[TCA_WRR_PARAMS]);

		if(wrr_copt==NULL) 
		{
			printk("wrr_change_class wrr_copt=%p",wrr_copt);
			return -EINVAL;
		}
		if(wrr_copt->quantum < 0 ) wrr_copt->quantum = 1500;
		if(wrr_copt->handle>0x10000 ||wrr_copt->handle <0)
			return -EINVAL; 

	}else
		return -EINVAL; 	

	err = -ENOBUFS;
	cl = kmalloc(sizeof(*cl), GFP_KERNEL);

	if (cl == NULL)
		goto failure;

	memset(cl, 0, sizeof(*cl));

	cl->refcnt = 1;

	if (!(cl->q = qdisc_create_dflt(qdisc_dev(sch),sch->dev_queue, &pfifo_qdisc_ops, classid)))
	{
		cl->q = &noop_qdisc;
		printk("wrr_change_class qdisc_create_dflt failed at dev %p queue:%p.\n",qdisc_dev(sch),cl->q);
	}

	cl->classid = classid;
	cl->quantum = wrr_copt->quantum;
	cl->deficit = cl->quantum;
	cl->nfmark = wrr_copt->handle;

	no = cl->nfmark % TOTALQUEUES;

	/* attach to the list */

	q->key = 0;
	q->total_classes ++;

	if(q->classes[no]!=NULL) return err;

	q->classes[no] = cl;	
	
	*arg = (unsigned long)cl;

#if WRR_DEBUG
    	printk("wrr_ch_class sch=%p, handle=%x, clsid=%x, parentid=%x no:%x quantum:%x nfmark: %x\n",
	    sch,sch->handle,classid,parentid,no,cl->quantum,cl->nfmark);
#endif

	return 0;

failure:
	return err;
}

static int 
wrr_delete(struct Qdisc *sch, unsigned long arg)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class *cl = (struct wrr_class*)arg;
	int i ;
   
	for(i=0;i< TOTALQUEUES;i++)
		if(q->classes[i]==cl) q->classes[i] = NULL;
  
	if (--cl->refcnt == 0)
		wrr_destroy_class(cl);

	return 0;
}

static struct tcf_proto **
wrr_find_tcf(struct Qdisc *sch, unsigned long arg)
{
#if WRR_DEBUG
	printk("wrr_find_tcf arg = %lx .\n",arg);
#endif
	return NULL;
}

static unsigned long 
wrr_bind_filter(struct Qdisc *sch, unsigned long parent,
				     u32 classid)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	
#if WRR_DEBUG
	printk("wrr_bind_filter parent %lx classid %x.\n",parent,classid);
#endif
	q->filters++;
	return 0;
}

static void 
wrr_unbind_filter(struct Qdisc *sch, unsigned long arg)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);

#if WRR_DEBUG
	printk("wrr_unbind_filter arg = %lx .\n",arg);
#endif
	q->filters--;
}

static void 
wrr_walk(struct Qdisc *sch, struct qdisc_walker *arg)
{
	struct wrr_sched_data *q = (struct wrr_sched_data *)qdisc_priv(sch);
	struct wrr_class **cl = q->classes;
	int i;

	if (arg->stop)
		return;

	for(i=0;i< TOTALQUEUES;i++){
	    if (arg->count < arg->skip) {
		arg->count++;
		continue;
	    }
	    if (arg->fn(sch, (unsigned long)cl[i], arg) < 0) {
		arg->stop = 1;
		return;
	    }
	    arg->count++;

	} 
}

static int 
wrr_dump_class_stats(struct Qdisc *sch, unsigned long arg, struct gnet_dump *d)
{
	struct wrr_class *cl = (struct wrr_class*)arg;

	cl->bstats.bytes = cl->stats.bytes;
	cl->bstats.packets = cl->stats.packets;
	cl->qstats.qlen = sch->q.qlen;
	cl->qstats.drops = cl->stats.drops;

	if (gnet_stats_copy_basic(d, &cl->bstats) < 0 ||
	    gnet_stats_copy_rate_est(d, &cl->rate_est) < 0 ||
	    gnet_stats_copy_queue(d, &cl->qstats) < 0)
		return -1;

	return 0;
}

static struct Qdisc_class_ops wrr_class_ops = {
	.graft		=	wrr_graft,
	.leaf		=	wrr_leaf,
	.get		=	wrr_get,
	.put		=	wrr_put,
	.change		=	wrr_change_class,
	.delete		=	wrr_delete,
	.walk		=	wrr_walk,
	.tcf_chain	=	wrr_find_tcf,
	.bind_tcf	=	wrr_bind_filter,
	.unbind_tcf	=	wrr_unbind_filter,
	.dump		=	wrr_dump_class,
	.dump_stats	=	wrr_dump_class_stats,
};

static struct Qdisc_ops wrr_qdisc_ops = {
	.cl_ops		=	&wrr_class_ops,
	.id		=	"wrr",
	.priv_size	=	sizeof(struct wrr_sched_data),
	.enqueue	=	wrr_enqueue,
	.dequeue	=	wrr_dequeue,
//	.requeue	=	wrr_requeue,
    .peek       =   qdisc_peek_dequeued,
	.drop		=	wrr_drop,
	.init		=	wrr_init,
	.reset		=	wrr_reset,
	.destroy	=	wrr_destroy,
	.change		=	NULL /* wrr_change */,
	.dump		=	wrr_dump,
	.owner		=	THIS_MODULE,
};


static int __init wrr_module_init(void)
{
    return register_qdisc(&wrr_qdisc_ops);
}
static void __exit wrr_module_exit(void) 
{
    unregister_qdisc(&wrr_qdisc_ops);
}

module_init(wrr_module_init)
module_exit(wrr_module_exit)