sch_htb.c

Linux Kernel 2.6.9 for OMAP1710

	rb_link_node(&cl->node[prio], parent, p);
	rb_insert_color(&cl->node[prio], root);
}

/**
 * htb_add_to_wait_tree - adds class to the event queue with delay
 *
 * The class is added to priority event queue to indicate that class will
 * change its mode in cl->pq_key microseconds. Make sure that class is not
 * already in the queue.
 */
static void htb_add_to_wait_tree (struct htb_sched *q,
		struct htb_class *cl,long delay,int debug_hint)
{
	struct rb_node **p = &q->wait_pq[cl->level].rb_node, *parent = NULL;
	HTB_DBG(7,3,"htb_add_wt cl=%X key=%lu\n",cl->classid,cl->pq_key);
#ifdef HTB_DEBUG
	if (cl->pq_node.rb_color != -1) { BUG_TRAP(0); return; }
	HTB_CHCL(cl);
	if ((delay <= 0 || delay > cl->mbuffer) && net_ratelimit())
		printk(KERN_ERR "HTB: suspicious delay in wait_tree d=%ld cl=%X h=%d\n",delay,cl->classid,debug_hint);
#endif
	cl->pq_key = q->jiffies + PSCHED_US2JIFFIE(delay);
	if (cl->pq_key == q->jiffies)
		cl->pq_key++;

	/* update the nearest event cache */
	if (time_after(q->near_ev_cache[cl->level], cl->pq_key))
		q->near_ev_cache[cl->level] = cl->pq_key;
	
	while (*p) {
		struct htb_class *c; parent = *p;
		c = rb_entry(parent, struct htb_class, pq_node);
		if (time_after_eq(cl->pq_key, c->pq_key))
			p = &parent->rb_right;
		else 
			p = &parent->rb_left;
	}
	rb_link_node(&cl->pq_node, parent, p);
	rb_insert_color(&cl->pq_node, &q->wait_pq[cl->level]);
}

/**
 * htb_next_rb_node - finds next node in binary tree
 *
 * When we are past last key we return NULL.
 * Average complexity is 2 steps per call.
 */
static void htb_next_rb_node(struct rb_node **n)
{
	*n = rb_next(*n);
}

/**
 * htb_add_class_to_row - add class to its row
 *
 * The class is added to row at priorities marked in mask.
 * It does nothing if mask == 0.
 */
static inline void htb_add_class_to_row(struct htb_sched *q, 
		struct htb_class *cl,int mask)
{
	HTB_DBG(7,2,"htb_addrow cl=%X mask=%X rmask=%X\n",
			cl->classid,mask,q->row_mask[cl->level]);
	HTB_CHCL(cl);
	q->row_mask[cl->level] |= mask;
	while (mask) {
		int prio = ffz(~mask);
		mask &= ~(1 << prio);
		htb_add_to_id_tree(HTB_PASSQ q->row[cl->level]+prio,cl,prio);
	}
}

/**
 * htb_remove_class_from_row - removes class from its row
 *
 * The class is removed from row at priorities marked in mask.
 * It does nothing if mask == 0.
 */
static __inline__ void htb_remove_class_from_row(struct htb_sched *q,
		struct htb_class *cl,int mask)
{
	int m = 0;
	HTB_CHCL(cl);
	while (mask) {
		int prio = ffz(~mask);
		mask &= ~(1 << prio);
		if (q->ptr[cl->level][prio] == cl->node+prio)
			htb_next_rb_node(q->ptr[cl->level]+prio);
		htb_safe_rb_erase(cl->node + prio,q->row[cl->level]+prio);
		if (!q->row[cl->level][prio].rb_node) 
			m |= 1 << prio;
	}
	HTB_DBG(7,2,"htb_delrow cl=%X mask=%X rmask=%X maskdel=%X\n",
			cl->classid,mask,q->row_mask[cl->level],m);
	q->row_mask[cl->level] &= ~m;
}

/**
 * htb_activate_prios - creates active classe's feed chain
 *
 * The class is connected to ancestors and/or appropriate rows
 * for priorities it is participating on. cl->cmode must be new 
 * (activated) mode. It does nothing if cl->prio_activity == 0.
 */
static void htb_activate_prios(struct htb_sched *q,struct htb_class *cl)
{
	struct htb_class *p = cl->parent;
	long m,mask = cl->prio_activity;
	HTB_DBG(7,2,"htb_act_prios cl=%X mask=%lX cmode=%d\n",cl->classid,mask,cl->cmode);
	HTB_CHCL(cl);

	while (cl->cmode == HTB_MAY_BORROW && p && mask) {
		HTB_CHCL(p);
		m = mask; while (m) {
			int prio = ffz(~m);
			m &= ~(1 << prio);
			
			if (p->un.inner.feed[prio].rb_node)
				/* parent already has its feed in use so that
				   reset bit in mask as parent is already ok */
				mask &= ~(1 << prio);
			
			htb_add_to_id_tree(HTB_PASSQ p->un.inner.feed+prio,cl,prio);
		}
		HTB_DBG(7,3,"htb_act_pr_aft p=%X pact=%X mask=%lX pmode=%d\n",
				p->classid,p->prio_activity,mask,p->cmode);
		p->prio_activity |= mask;
		cl = p; p = cl->parent;
		HTB_CHCL(cl);
	}
	if (cl->cmode == HTB_CAN_SEND && mask)
		htb_add_class_to_row(q,cl,mask);
}

/**
 * htb_deactivate_prios - remove class from feed chain
 *
 * cl->cmode must represent old mode (before deactivation). It does 
 * nothing if cl->prio_activity == 0. Class is removed from all feed
 * chains and rows.
 */
static void htb_deactivate_prios(struct htb_sched *q, struct htb_class *cl)
{
	struct htb_class *p = cl->parent;
	long m,mask = cl->prio_activity;
	HTB_DBG(7,2,"htb_deact_prios cl=%X mask=%lX cmode=%d\n",cl->classid,mask,cl->cmode);
	HTB_CHCL(cl);

	while (cl->cmode == HTB_MAY_BORROW && p && mask) {
		m = mask; mask = 0; 
		while (m) {
			int prio = ffz(~m);
			m &= ~(1 << prio);
			
			if (p->un.inner.ptr[prio] == cl->node+prio) {
				/* we are removing child which is pointed to from
				   parent feed - forget the pointer but remember
				   classid */
				p->un.inner.last_ptr_id[prio] = cl->classid;
				p->un.inner.ptr[prio] = NULL;
			}
			
			htb_safe_rb_erase(cl->node + prio,p->un.inner.feed + prio);
			
			if (!p->un.inner.feed[prio].rb_node) 
				mask |= 1 << prio;
		}
		HTB_DBG(7,3,"htb_deact_pr_aft p=%X pact=%X mask=%lX pmode=%d\n",
				p->classid,p->prio_activity,mask,p->cmode);
		p->prio_activity &= ~mask;
		cl = p; p = cl->parent;
		HTB_CHCL(cl);
	}
	if (cl->cmode == HTB_CAN_SEND && mask) 
		htb_remove_class_from_row(q,cl,mask);
}

/**
 * htb_class_mode - computes and returns current class mode
 *
 * It computes cl's mode at time cl->t_c+diff and returns it. If mode
 * is not HTB_CAN_SEND then cl->pq_key is updated to time difference
 * from now to time when cl will change its state. 
 * Also it is worth to note that class mode doesn't change simply
 * at cl->{c,}tokens == 0 but there can rather be hysteresis of 
 * 0 .. -cl->{c,}buffer range. It is meant to limit number of
 * mode transitions per time unit. The speed gain is about 1/6.
 */
static __inline__ enum htb_cmode 
htb_class_mode(struct htb_class *cl,long *diff)
{
    long toks;

    if ((toks = (cl->ctokens + *diff)) < (
#if HTB_HYSTERESIS
	    cl->cmode != HTB_CANT_SEND ? -cl->cbuffer :
#endif
       	    0)) {
	    *diff = -toks;
	    return HTB_CANT_SEND;
    }
    if ((toks = (cl->tokens + *diff)) >= (
#if HTB_HYSTERESIS
	    cl->cmode == HTB_CAN_SEND ? -cl->buffer :
#endif
	    0))
	    return HTB_CAN_SEND;

    *diff = -toks;
    return HTB_MAY_BORROW;
}

/**
 * htb_change_class_mode - changes classe's mode
 *
 * This should be the only way how to change classe's mode under normal
 * cirsumstances. Routine will update feed lists linkage, change mode
 * and add class to the wait event queue if appropriate. New mode should
 * be different from old one and cl->pq_key has to be valid if changing
 * to mode other than HTB_CAN_SEND (see htb_add_to_wait_tree).
 */
static void 
htb_change_class_mode(struct htb_sched *q, struct htb_class *cl, long *diff)
{ 
	enum htb_cmode new_mode = htb_class_mode(cl,diff);
	
	HTB_CHCL(cl);
	HTB_DBG(7,1,"htb_chging_clmode %d->%d cl=%X\n",cl->cmode,new_mode,cl->classid);

	if (new_mode == cl->cmode)
		return;	
	
	if (cl->prio_activity) { /* not necessary: speed optimization */
		if (cl->cmode != HTB_CANT_SEND) 
			htb_deactivate_prios(q,cl);
		cl->cmode = new_mode;
		if (new_mode != HTB_CANT_SEND) 
			htb_activate_prios(q,cl);
	} else 
		cl->cmode = new_mode;
}

/**
 * htb_activate - inserts leaf cl into appropriate active feeds 
 *
 * Routine learns (new) priority of leaf and activates feed chain
 * for the prio. It can be called on already active leaf safely.
 * It also adds leaf into droplist.
 */
static __inline__ void htb_activate(struct htb_sched *q,struct htb_class *cl)
{
	BUG_TRAP(!cl->level && cl->un.leaf.q && cl->un.leaf.q->q.qlen);
	HTB_CHCL(cl);
	if (!cl->prio_activity) {
		cl->prio_activity = 1 << (cl->un.leaf.aprio = cl->un.leaf.prio);
		htb_activate_prios(q,cl);
		list_add_tail(&cl->un.leaf.drop_list,q->drops+cl->un.leaf.aprio);
	}
}

/**
 * htb_deactivate - remove leaf cl from active feeds 
 *
 * Make sure that leaf is active. In the other words it can't be called
 * with non-active leaf. It also removes class from the drop list.
 */
static __inline__ void 
htb_deactivate(struct htb_sched *q,struct htb_class *cl)
{
	BUG_TRAP(cl->prio_activity);
	HTB_CHCL(cl);
	htb_deactivate_prios(q,cl);
	cl->prio_activity = 0;
	list_del_init(&cl->un.leaf.drop_list);
}

static int htb_enqueue(struct sk_buff *skb, struct Qdisc *sch)
{
    int ret = NET_XMIT_SUCCESS;
    struct htb_sched *q = qdisc_priv(sch);
    struct htb_class *cl = htb_classify(skb,sch,&ret);


#ifdef CONFIG_NET_CLS_ACT
    if (cl == HTB_DIRECT ) {
	if (q->direct_queue.qlen < q->direct_qlen ) {
	    __skb_queue_tail(&q->direct_queue, skb);
	    q->direct_pkts++;
	}
    } else if (!cl) {
	    if (NET_XMIT_DROP == ret) {
		    sch->stats.drops++;
	    }
	    return ret;
    }
#else
    if (cl == HTB_DIRECT || !cl) {
	/* enqueue to helper queue */
	if (q->direct_queue.qlen < q->direct_qlen && cl) {
	    __skb_queue_tail(&q->direct_queue, skb);
	    q->direct_pkts++;
	} else {
	    kfree_skb (skb);
	    sch->stats.drops++;
	    return NET_XMIT_DROP;
	}
    }
#endif
    else if (cl->un.leaf.q->enqueue(skb, cl->un.leaf.q) != NET_XMIT_SUCCESS) {
	sch->stats.drops++;
	cl->stats.drops++;
	return NET_XMIT_DROP;
    } else {
	cl->stats.packets++; cl->stats.bytes += skb->len;
	htb_activate (q,cl);
    }

    sch->q.qlen++;
    sch->stats.packets++; sch->stats.bytes += skb->len;
    HTB_DBG(1,1,"htb_enq_ok cl=%X skb=%p\n",(cl && cl != HTB_DIRECT)?cl->classid:0,skb);
    return NET_XMIT_SUCCESS;
}

/* TODO: requeuing packet charges it to policers again !! */
static int htb_requeue(struct sk_buff *skb, struct Qdisc *sch)
{
    struct htb_sched *q = qdisc_priv(sch);
    int ret =  NET_XMIT_SUCCESS;
    struct htb_class *cl = htb_classify(skb,sch, &ret);
    struct sk_buff *tskb;

    if (cl == HTB_DIRECT || !cl) {
	/* enqueue to helper queue */
	if (q->direct_queue.qlen < q->direct_qlen && cl) {
	    __skb_queue_head(&q->direct_queue, skb);
	} else {
            __skb_queue_head(&q->direct_queue, skb);
            tskb = __skb_dequeue_tail(&q->direct_queue);
            kfree_skb (tskb);
            sch->stats.drops++;
            return NET_XMIT_CN;	
	}
    } else if (cl->un.leaf.q->ops->requeue(skb, cl->un.leaf.q) != NET_XMIT_SUCCESS) {
	sch->stats.drops++;
	cl->stats.drops++;
	return NET_XMIT_DROP;
    } else 
	    htb_activate (q,cl);

    sch->q.qlen++;
    HTB_DBG(1,1,"htb_req_ok cl=%X skb=%p\n",(cl && cl != HTB_DIRECT)?cl->classid:0,skb);
    return NET_XMIT_SUCCESS;
}

static void htb_timer(unsigned long arg)
{
    struct Qdisc *sch = (struct Qdisc*)arg;
    sch->flags &= ~TCQ_F_THROTTLED;
    wmb();
    netif_schedule(sch->dev);
}

#ifdef HTB_RATECM
#define RT_GEN(D,R) R+=D-(R/HTB_EWMAC);D=0
static void htb_rate_timer(unsigned long arg)
{
	struct Qdisc *sch = (struct Qdisc*)arg;
	struct htb_sched *q = qdisc_priv(sch);
	struct list_head *p;

	/* lock queue so that we can muck with it */
	HTB_QLOCK(sch);
	HTB_DBG(10,1,"htb_rttmr j=%ld\n",jiffies);

	q->rttim.expires = jiffies + HZ;
	add_timer(&q->rttim);

	/* scan and recompute one bucket at time */
	if (++q->recmp_bucket >= HTB_HSIZE) 
		q->recmp_bucket = 0;
	list_for_each (p,q->hash+q->recmp_bucket) {
		struct htb_class *cl = list_entry(p,struct htb_class,hlist);
		HTB_DBG(10,2,"htb_rttmr_cl cl=%X sbyte=%lu spkt=%lu\n",
				cl->classid,cl->sum_bytes,cl->sum_packets);
		RT_GEN (cl->sum_bytes,cl->rate_bytes);
		RT_GEN (cl->sum_packets,cl->rate_packets);
	}
	HTB_QUNLOCK(sch);
}
#endif

/**
 * htb_charge_class - charges amount "bytes" to leaf and ancestors
 *
 * Routine assumes that packet "bytes" long was dequeued from leaf cl
 * borrowing from "level". It accounts bytes to ceil leaky bucket for
 * leaf and all ancestors and to rate bucket for ancestors at levels
 * "level" and higher. It also handles possible change of mode resulting
 * from the update. Note that mode can also increase here (MAY_BORROW to
 * CAN_SEND) because we can use more precise clock that event queue here.
 * In such case we remove class from event queue first.
 */
static void htb_charge_class(struct htb_sched *q,struct htb_class *cl,
		int level,int bytes)
{	
	long toks,diff;
	enum htb_cmode old_mode;
	HTB_DBG(5,1,"htb_chrg_cl cl=%X lev=%d len=%d\n",cl->classid,level,bytes);

#define HTB_ACCNT(T,B,R) toks = diff + cl->T; \
	if (toks > cl->B) toks = cl->B; \
	toks -= L2T(cl, cl->R, bytes); \
	if (toks <= -cl->mbuffer) toks = 1-cl->mbuffer; \
	cl->T = toks

	while (cl) {
		HTB_CHCL(cl);
		diff = PSCHED_TDIFF_SAFE(q->now, cl->t_c, (u32)cl->mbuffer);
#ifdef HTB_DEBUG
		if (diff > cl->mbuffer || diff < 0 || PSCHED_TLESS(q->now, cl->t_c)) {
			if (net_ratelimit())
				printk(KERN_ERR "HTB: bad diff in charge, cl=%X diff=%lX now=%Lu then=%Lu j=%lu\n",
				       cl->classid, diff,
#ifdef CONFIG_NET_SCH_CLK_GETTIMEOFDAY
				       q->now.tv_sec * 1000000ULL + q->now.tv_usec,
				       cl->t_c.tv_sec * 1000000ULL + cl->t_c.tv_usec,
#else
				       (unsigned long long) q->now,
				       (unsigned long long) cl->t_c,
#endif
				       q->jiffies);
			diff = 1000;
		}
#endif
		if (cl->level >= level) {
			if (cl->level == level) cl->xstats.lends++;
			HTB_ACCNT (tokens,buffer,rate);
		} else {
			cl->xstats.borrows++;
			cl->tokens += diff; /* we moved t_c; update tokens */
		}
		HTB_ACCNT (ctokens,cbuffer,ceil);
		cl->t_c = q->now;
		HTB_DBG(5,2,"htb_chrg_clp cl=%X diff=%ld tok=%ld ctok=%ld\n",cl->classid,diff,cl->tokens,cl->ctokens);