[NET_SCHED]: Propagate nla_parse return value
[linux-2.6.git] / net / sched / sch_htb.c
1 /*
2  * net/sched/sch_htb.c  Hierarchical token bucket, feed tree version
3  *
4  *              This program is free software; you can redistribute it and/or
5  *              modify it under the terms of the GNU General Public License
6  *              as published by the Free Software Foundation; either version
7  *              2 of the License, or (at your option) any later version.
8  *
9  * Authors:     Martin Devera, <devik@cdi.cz>
10  *
11  * Credits (in time order) for older HTB versions:
12  *              Stef Coene <stef.coene@docum.org>
13  *                      HTB support at LARTC mailing list
14  *              Ondrej Kraus, <krauso@barr.cz>
15  *                      found missing INIT_QDISC(htb)
16  *              Vladimir Smelhaus, Aamer Akhter, Bert Hubert
17  *                      helped a lot to locate nasty class stall bug
18  *              Andi Kleen, Jamal Hadi, Bert Hubert
19  *                      code review and helpful comments on shaping
20  *              Tomasz Wrona, <tw@eter.tym.pl>
21  *                      created test case so that I was able to fix nasty bug
22  *              Wilfried Weissmann
23  *                      spotted bug in dequeue code and helped with fix
24  *              Jiri Fojtasek
25  *                      fixed requeue routine
26  *              and many others. thanks.
27  *
28  * $Id: sch_htb.c,v 1.25 2003/12/07 11:08:25 devik Exp devik $
29  */
30 #include <linux/module.h>
31 #include <linux/types.h>
32 #include <linux/kernel.h>
33 #include <linux/string.h>
34 #include <linux/errno.h>
35 #include <linux/skbuff.h>
36 #include <linux/list.h>
37 #include <linux/compiler.h>
38 #include <linux/rbtree.h>
39 #include <net/netlink.h>
40 #include <net/pkt_sched.h>
41
42 /* HTB algorithm.
43     Author: devik@cdi.cz
44     ========================================================================
45     HTB is like TBF with multiple classes. It is also similar to CBQ because
46     it allows to assign priority to each class in hierarchy.
47     In fact it is another implementation of Floyd's formal sharing.
48
49     Levels:
50     Each class is assigned level. Leaf has ALWAYS level 0 and root
51     classes have level TC_HTB_MAXDEPTH-1. Interior nodes has level
52     one less than their parent.
53 */
54
55 #define HTB_HSIZE 16            /* classid hash size */
56 #define HTB_HYSTERESIS 1        /* whether to use mode hysteresis for speedup */
57 #define HTB_VER 0x30011         /* major must be matched with number suplied by TC as version */
58
59 #if HTB_VER >> 16 != TC_HTB_PROTOVER
60 #error "Mismatched sch_htb.c and pkt_sch.h"
61 #endif
62
63 /* used internaly to keep status of single class */
64 enum htb_cmode {
65         HTB_CANT_SEND,          /* class can't send and can't borrow */
66         HTB_MAY_BORROW,         /* class can't send but may borrow */
67         HTB_CAN_SEND            /* class can send */
68 };
69
70 /* interior & leaf nodes; props specific to leaves are marked L: */
71 struct htb_class {
72         /* general class parameters */
73         u32 classid;
74         struct gnet_stats_basic bstats;
75         struct gnet_stats_queue qstats;
76         struct gnet_stats_rate_est rate_est;
77         struct tc_htb_xstats xstats;    /* our special stats */
78         int refcnt;             /* usage count of this class */
79
80         /* topology */
81         int level;              /* our level (see above) */
82         struct htb_class *parent;       /* parent class */
83         struct hlist_node hlist;        /* classid hash list item */
84         struct list_head sibling;       /* sibling list item */
85         struct list_head children;      /* children list */
86
87         union {
88                 struct htb_class_leaf {
89                         struct Qdisc *q;
90                         int prio;
91                         int aprio;
92                         int quantum;
93                         int deficit[TC_HTB_MAXDEPTH];
94                         struct list_head drop_list;
95                 } leaf;
96                 struct htb_class_inner {
97                         struct rb_root feed[TC_HTB_NUMPRIO];    /* feed trees */
98                         struct rb_node *ptr[TC_HTB_NUMPRIO];    /* current class ptr */
99                         /* When class changes from state 1->2 and disconnects from
100                            parent's feed then we lost ptr value and start from the
101                            first child again. Here we store classid of the
102                            last valid ptr (used when ptr is NULL). */
103                         u32 last_ptr_id[TC_HTB_NUMPRIO];
104                 } inner;
105         } un;
106         struct rb_node node[TC_HTB_NUMPRIO];    /* node for self or feed tree */
107         struct rb_node pq_node; /* node for event queue */
108         psched_time_t pq_key;
109
110         int prio_activity;      /* for which prios are we active */
111         enum htb_cmode cmode;   /* current mode of the class */
112
113         /* class attached filters */
114         struct tcf_proto *filter_list;
115         int filter_cnt;
116
117         int warned;             /* only one warning about non work conserving .. */
118
119         /* token bucket parameters */
120         struct qdisc_rate_table *rate;  /* rate table of the class itself */
121         struct qdisc_rate_table *ceil;  /* ceiling rate (limits borrows too) */
122         long buffer, cbuffer;   /* token bucket depth/rate */
123         psched_tdiff_t mbuffer; /* max wait time */
124         long tokens, ctokens;   /* current number of tokens */
125         psched_time_t t_c;      /* checkpoint time */
126
127         int prio;               /* For parent to leaf return possible here */
128         int quantum;            /* we do backup. Finally full replacement  */
129                                 /* of un.leaf originals should be done. */
130 };
131
132 static inline long L2T(struct htb_class *cl, struct qdisc_rate_table *rate,
133                            int size)
134 {
135         long result = qdisc_l2t(rate, size);
136         return result;
137 }
138
139 struct htb_sched {
140         struct list_head root;  /* root classes list */
141         struct hlist_head hash[HTB_HSIZE];      /* hashed by classid */
142         struct list_head drops[TC_HTB_NUMPRIO];/* active leaves (for drops) */
143
144         /* self list - roots of self generating tree */
145         struct rb_root row[TC_HTB_MAXDEPTH][TC_HTB_NUMPRIO];
146         int row_mask[TC_HTB_MAXDEPTH];
147         struct rb_node *ptr[TC_HTB_MAXDEPTH][TC_HTB_NUMPRIO];
148         u32 last_ptr_id[TC_HTB_MAXDEPTH][TC_HTB_NUMPRIO];
149
150         /* self wait list - roots of wait PQs per row */
151         struct rb_root wait_pq[TC_HTB_MAXDEPTH];
152
153         /* time of nearest event per level (row) */
154         psched_time_t near_ev_cache[TC_HTB_MAXDEPTH];
155
156         /* whether we hit non-work conserving class during this dequeue; we use */
157         int nwc_hit;            /* this to disable mindelay complaint in dequeue */
158
159         int defcls;             /* class where unclassified flows go to */
160
161         /* filters for qdisc itself */
162         struct tcf_proto *filter_list;
163         int filter_cnt;
164
165         int rate2quantum;       /* quant = rate / rate2quantum */
166         psched_time_t now;      /* cached dequeue time */
167         struct qdisc_watchdog watchdog;
168
169         /* non shaped skbs; let them go directly thru */
170         struct sk_buff_head direct_queue;
171         int direct_qlen;        /* max qlen of above */
172
173         long direct_pkts;
174 };
175
176 /* compute hash of size HTB_HSIZE for given handle */
177 static inline int htb_hash(u32 h)
178 {
179 #if HTB_HSIZE != 16
180 #error "Declare new hash for your HTB_HSIZE"
181 #endif
182         h ^= h >> 8;            /* stolen from cbq_hash */
183         h ^= h >> 4;
184         return h & 0xf;
185 }
186
187 /* find class in global hash table using given handle */
188 static inline struct htb_class *htb_find(u32 handle, struct Qdisc *sch)
189 {
190         struct htb_sched *q = qdisc_priv(sch);
191         struct hlist_node *p;
192         struct htb_class *cl;
193
194         if (TC_H_MAJ(handle) != sch->handle)
195                 return NULL;
196
197         hlist_for_each_entry(cl, p, q->hash + htb_hash(handle), hlist) {
198                 if (cl->classid == handle)
199                         return cl;
200         }
201         return NULL;
202 }
203
204 /**
205  * htb_classify - classify a packet into class
206  *
207  * It returns NULL if the packet should be dropped or -1 if the packet
208  * should be passed directly thru. In all other cases leaf class is returned.
209  * We allow direct class selection by classid in priority. The we examine
210  * filters in qdisc and in inner nodes (if higher filter points to the inner
211  * node). If we end up with classid MAJOR:0 we enqueue the skb into special
212  * internal fifo (direct). These packets then go directly thru. If we still
213  * have no valid leaf we try to use MAJOR:default leaf. It still unsuccessfull
214  * then finish and return direct queue.
215  */
216 #define HTB_DIRECT (struct htb_class*)-1
217
218 static struct htb_class *htb_classify(struct sk_buff *skb, struct Qdisc *sch,
219                                       int *qerr)
220 {
221         struct htb_sched *q = qdisc_priv(sch);
222         struct htb_class *cl;
223         struct tcf_result res;
224         struct tcf_proto *tcf;
225         int result;
226
227         /* allow to select class by setting skb->priority to valid classid;
228            note that nfmark can be used too by attaching filter fw with no
229            rules in it */
230         if (skb->priority == sch->handle)
231                 return HTB_DIRECT;      /* X:0 (direct flow) selected */
232         if ((cl = htb_find(skb->priority, sch)) != NULL && cl->level == 0)
233                 return cl;
234
235         *qerr = NET_XMIT_BYPASS;
236         tcf = q->filter_list;
237         while (tcf && (result = tc_classify(skb, tcf, &res)) >= 0) {
238 #ifdef CONFIG_NET_CLS_ACT
239                 switch (result) {
240                 case TC_ACT_QUEUED:
241                 case TC_ACT_STOLEN:
242                         *qerr = NET_XMIT_SUCCESS;
243                 case TC_ACT_SHOT:
244                         return NULL;
245                 }
246 #endif
247                 if ((cl = (void *)res.class) == NULL) {
248                         if (res.classid == sch->handle)
249                                 return HTB_DIRECT;      /* X:0 (direct flow) */
250                         if ((cl = htb_find(res.classid, sch)) == NULL)
251                                 break;  /* filter selected invalid classid */
252                 }
253                 if (!cl->level)
254                         return cl;      /* we hit leaf; return it */
255
256                 /* we have got inner class; apply inner filter chain */
257                 tcf = cl->filter_list;
258         }
259         /* classification failed; try to use default class */
260         cl = htb_find(TC_H_MAKE(TC_H_MAJ(sch->handle), q->defcls), sch);
261         if (!cl || cl->level)
262                 return HTB_DIRECT;      /* bad default .. this is safe bet */
263         return cl;
264 }
265
266 /**
267  * htb_add_to_id_tree - adds class to the round robin list
268  *
269  * Routine adds class to the list (actually tree) sorted by classid.
270  * Make sure that class is not already on such list for given prio.
271  */
272 static void htb_add_to_id_tree(struct rb_root *root,
273                                struct htb_class *cl, int prio)
274 {
275         struct rb_node **p = &root->rb_node, *parent = NULL;
276
277         while (*p) {
278                 struct htb_class *c;
279                 parent = *p;
280                 c = rb_entry(parent, struct htb_class, node[prio]);
281
282                 if (cl->classid > c->classid)
283                         p = &parent->rb_right;
284                 else
285                         p = &parent->rb_left;
286         }
287         rb_link_node(&cl->node[prio], parent, p);
288         rb_insert_color(&cl->node[prio], root);
289 }
290
291 /**
292  * htb_add_to_wait_tree - adds class to the event queue with delay
293  *
294  * The class is added to priority event queue to indicate that class will
295  * change its mode in cl->pq_key microseconds. Make sure that class is not
296  * already in the queue.
297  */
298 static void htb_add_to_wait_tree(struct htb_sched *q,
299                                  struct htb_class *cl, long delay)
300 {
301         struct rb_node **p = &q->wait_pq[cl->level].rb_node, *parent = NULL;
302
303         cl->pq_key = q->now + delay;
304         if (cl->pq_key == q->now)
305                 cl->pq_key++;
306
307         /* update the nearest event cache */
308         if (q->near_ev_cache[cl->level] > cl->pq_key)
309                 q->near_ev_cache[cl->level] = cl->pq_key;
310
311         while (*p) {
312                 struct htb_class *c;
313                 parent = *p;
314                 c = rb_entry(parent, struct htb_class, pq_node);
315                 if (cl->pq_key >= c->pq_key)
316                         p = &parent->rb_right;
317                 else
318                         p = &parent->rb_left;
319         }
320         rb_link_node(&cl->pq_node, parent, p);
321         rb_insert_color(&cl->pq_node, &q->wait_pq[cl->level]);
322 }
323
324 /**
325  * htb_next_rb_node - finds next node in binary tree
326  *
327  * When we are past last key we return NULL.
328  * Average complexity is 2 steps per call.
329  */
330 static inline void htb_next_rb_node(struct rb_node **n)
331 {
332         *n = rb_next(*n);
333 }
334
335 /**
336  * htb_add_class_to_row - add class to its row
337  *
338  * The class is added to row at priorities marked in mask.
339  * It does nothing if mask == 0.
340  */
341 static inline void htb_add_class_to_row(struct htb_sched *q,
342                                         struct htb_class *cl, int mask)
343 {
344         q->row_mask[cl->level] |= mask;
345         while (mask) {
346                 int prio = ffz(~mask);
347                 mask &= ~(1 << prio);
348                 htb_add_to_id_tree(q->row[cl->level] + prio, cl, prio);
349         }
350 }
351
352 /* If this triggers, it is a bug in this code, but it need not be fatal */
353 static void htb_safe_rb_erase(struct rb_node *rb, struct rb_root *root)
354 {
355         if (RB_EMPTY_NODE(rb)) {
356                 WARN_ON(1);
357         } else {
358                 rb_erase(rb, root);
359                 RB_CLEAR_NODE(rb);
360         }
361 }
362
363
364 /**
365  * htb_remove_class_from_row - removes class from its row
366  *
367  * The class is removed from row at priorities marked in mask.
368  * It does nothing if mask == 0.
369  */
370 static inline void htb_remove_class_from_row(struct htb_sched *q,
371                                                  struct htb_class *cl, int mask)
372 {
373         int m = 0;
374
375         while (mask) {
376                 int prio = ffz(~mask);
377
378                 mask &= ~(1 << prio);
379                 if (q->ptr[cl->level][prio] == cl->node + prio)
380                         htb_next_rb_node(q->ptr[cl->level] + prio);
381
382                 htb_safe_rb_erase(cl->node + prio, q->row[cl->level] + prio);
383                 if (!q->row[cl->level][prio].rb_node)
384                         m |= 1 << prio;
385         }
386         q->row_mask[cl->level] &= ~m;
387 }
388
389 /**
390  * htb_activate_prios - creates active classe's feed chain
391  *
392  * The class is connected to ancestors and/or appropriate rows
393  * for priorities it is participating on. cl->cmode must be new
394  * (activated) mode. It does nothing if cl->prio_activity == 0.
395  */
396 static void htb_activate_prios(struct htb_sched *q, struct htb_class *cl)
397 {
398         struct htb_class *p = cl->parent;
399         long m, mask = cl->prio_activity;
400
401         while (cl->cmode == HTB_MAY_BORROW && p && mask) {
402                 m = mask;
403                 while (m) {
404                         int prio = ffz(~m);
405                         m &= ~(1 << prio);
406
407                         if (p->un.inner.feed[prio].rb_node)
408                                 /* parent already has its feed in use so that
409                                    reset bit in mask as parent is already ok */
410                                 mask &= ~(1 << prio);
411
412                         htb_add_to_id_tree(p->un.inner.feed + prio, cl, prio);
413                 }
414                 p->prio_activity |= mask;
415                 cl = p;
416                 p = cl->parent;
417
418         }
419         if (cl->cmode == HTB_CAN_SEND && mask)
420                 htb_add_class_to_row(q, cl, mask);
421 }
422
423 /**
424  * htb_deactivate_prios - remove class from feed chain
425  *
426  * cl->cmode must represent old mode (before deactivation). It does
427  * nothing if cl->prio_activity == 0. Class is removed from all feed
428  * chains and rows.
429  */
430 static void htb_deactivate_prios(struct htb_sched *q, struct htb_class *cl)
431 {
432         struct htb_class *p = cl->parent;
433         long m, mask = cl->prio_activity;
434
435         while (cl->cmode == HTB_MAY_BORROW && p && mask) {
436                 m = mask;
437                 mask = 0;
438                 while (m) {
439                         int prio = ffz(~m);
440                         m &= ~(1 << prio);
441
442                         if (p->un.inner.ptr[prio] == cl->node + prio) {
443                                 /* we are removing child which is pointed to from
444                                    parent feed - forget the pointer but remember
445                                    classid */
446                                 p->un.inner.last_ptr_id[prio] = cl->classid;
447                                 p->un.inner.ptr[prio] = NULL;
448                         }
449
450                         htb_safe_rb_erase(cl->node + prio, p->un.inner.feed + prio);
451
452                         if (!p->un.inner.feed[prio].rb_node)
453                                 mask |= 1 << prio;
454                 }
455
456                 p->prio_activity &= ~mask;
457                 cl = p;
458                 p = cl->parent;
459
460         }
461         if (cl->cmode == HTB_CAN_SEND && mask)
462                 htb_remove_class_from_row(q, cl, mask);
463 }
464
465 #if HTB_HYSTERESIS
466 static inline long htb_lowater(const struct htb_class *cl)
467 {
468         return cl->cmode != HTB_CANT_SEND ? -cl->cbuffer : 0;
469 }
470 static inline long htb_hiwater(const struct htb_class *cl)
471 {
472         return cl->cmode == HTB_CAN_SEND ? -cl->buffer : 0;
473 }
474 #else
475 #define htb_lowater(cl) (0)
476 #define htb_hiwater(cl) (0)
477 #endif
478
479 /**
480  * htb_class_mode - computes and returns current class mode
481  *
482  * It computes cl's mode at time cl->t_c+diff and returns it. If mode
483  * is not HTB_CAN_SEND then cl->pq_key is updated to time difference
484  * from now to time when cl will change its state.
485  * Also it is worth to note that class mode doesn't change simply
486  * at cl->{c,}tokens == 0 but there can rather be hysteresis of
487  * 0 .. -cl->{c,}buffer range. It is meant to limit number of
488  * mode transitions per time unit. The speed gain is about 1/6.
489  */
490 static inline enum htb_cmode
491 htb_class_mode(struct htb_class *cl, long *diff)
492 {
493         long toks;
494
495         if ((toks = (cl->ctokens + *diff)) < htb_lowater(cl)) {
496                 *diff = -toks;
497                 return HTB_CANT_SEND;
498         }
499
500         if ((toks = (cl->tokens + *diff)) >= htb_hiwater(cl))
501                 return HTB_CAN_SEND;
502
503         *diff = -toks;
504         return HTB_MAY_BORROW;
505 }
506
507 /**
508  * htb_change_class_mode - changes classe's mode
509  *
510  * This should be the only way how to change classe's mode under normal
511  * cirsumstances. Routine will update feed lists linkage, change mode
512  * and add class to the wait event queue if appropriate. New mode should
513  * be different from old one and cl->pq_key has to be valid if changing
514  * to mode other than HTB_CAN_SEND (see htb_add_to_wait_tree).
515  */
516 static void
517 htb_change_class_mode(struct htb_sched *q, struct htb_class *cl, long *diff)
518 {
519         enum htb_cmode new_mode = htb_class_mode(cl, diff);
520
521         if (new_mode == cl->cmode)
522                 return;
523
524         if (cl->prio_activity) {        /* not necessary: speed optimization */
525                 if (cl->cmode != HTB_CANT_SEND)
526                         htb_deactivate_prios(q, cl);
527                 cl->cmode = new_mode;
528                 if (new_mode != HTB_CANT_SEND)
529                         htb_activate_prios(q, cl);
530         } else
531                 cl->cmode = new_mode;
532 }
533
534 /**
535  * htb_activate - inserts leaf cl into appropriate active feeds
536  *
537  * Routine learns (new) priority of leaf and activates feed chain
538  * for the prio. It can be called on already active leaf safely.
539  * It also adds leaf into droplist.
540  */
541 static inline void htb_activate(struct htb_sched *q, struct htb_class *cl)
542 {
543         BUG_TRAP(!cl->level && cl->un.leaf.q && cl->un.leaf.q->q.qlen);
544
545         if (!cl->prio_activity) {
546                 cl->prio_activity = 1 << (cl->un.leaf.aprio = cl->un.leaf.prio);
547                 htb_activate_prios(q, cl);
548                 list_add_tail(&cl->un.leaf.drop_list,
549                               q->drops + cl->un.leaf.aprio);
550         }
551 }
552
553 /**
554  * htb_deactivate - remove leaf cl from active feeds
555  *
556  * Make sure that leaf is active. In the other words it can't be called
557  * with non-active leaf. It also removes class from the drop list.
558  */
559 static inline void htb_deactivate(struct htb_sched *q, struct htb_class *cl)
560 {
561         BUG_TRAP(cl->prio_activity);
562
563         htb_deactivate_prios(q, cl);
564         cl->prio_activity = 0;
565         list_del_init(&cl->un.leaf.drop_list);
566 }
567
568 static int htb_enqueue(struct sk_buff *skb, struct Qdisc *sch)
569 {
570         int ret;
571         struct htb_sched *q = qdisc_priv(sch);
572         struct htb_class *cl = htb_classify(skb, sch, &ret);
573
574         if (cl == HTB_DIRECT) {
575                 /* enqueue to helper queue */
576                 if (q->direct_queue.qlen < q->direct_qlen) {
577                         __skb_queue_tail(&q->direct_queue, skb);
578                         q->direct_pkts++;
579                 } else {
580                         kfree_skb(skb);
581                         sch->qstats.drops++;
582                         return NET_XMIT_DROP;
583                 }
584 #ifdef CONFIG_NET_CLS_ACT
585         } else if (!cl) {
586                 if (ret == NET_XMIT_BYPASS)
587                         sch->qstats.drops++;
588                 kfree_skb(skb);
589                 return ret;
590 #endif
591         } else if (cl->un.leaf.q->enqueue(skb, cl->un.leaf.q) !=
592                    NET_XMIT_SUCCESS) {
593                 sch->qstats.drops++;
594                 cl->qstats.drops++;
595                 return NET_XMIT_DROP;
596         } else {
597                 cl->bstats.packets +=
598                         skb_is_gso(skb)?skb_shinfo(skb)->gso_segs:1;
599                 cl->bstats.bytes += skb->len;
600                 htb_activate(q, cl);
601         }
602
603         sch->q.qlen++;
604         sch->bstats.packets += skb_is_gso(skb)?skb_shinfo(skb)->gso_segs:1;
605         sch->bstats.bytes += skb->len;
606         return NET_XMIT_SUCCESS;
607 }
608
609 /* TODO: requeuing packet charges it to policers again !! */
610 static int htb_requeue(struct sk_buff *skb, struct Qdisc *sch)
611 {
612         struct htb_sched *q = qdisc_priv(sch);
613         int ret = NET_XMIT_SUCCESS;
614         struct htb_class *cl = htb_classify(skb, sch, &ret);
615         struct sk_buff *tskb;
616
617         if (cl == HTB_DIRECT || !cl) {
618                 /* enqueue to helper queue */
619                 if (q->direct_queue.qlen < q->direct_qlen && cl) {
620                         __skb_queue_head(&q->direct_queue, skb);
621                 } else {
622                         __skb_queue_head(&q->direct_queue, skb);
623                         tskb = __skb_dequeue_tail(&q->direct_queue);
624                         kfree_skb(tskb);
625                         sch->qstats.drops++;
626                         return NET_XMIT_CN;
627                 }
628         } else if (cl->un.leaf.q->ops->requeue(skb, cl->un.leaf.q) !=
629                    NET_XMIT_SUCCESS) {
630                 sch->qstats.drops++;
631                 cl->qstats.drops++;
632                 return NET_XMIT_DROP;
633         } else
634                 htb_activate(q, cl);
635
636         sch->q.qlen++;
637         sch->qstats.requeues++;
638         return NET_XMIT_SUCCESS;
639 }
640
641 /**
642  * htb_charge_class - charges amount "bytes" to leaf and ancestors
643  *
644  * Routine assumes that packet "bytes" long was dequeued from leaf cl
645  * borrowing from "level". It accounts bytes to ceil leaky bucket for
646  * leaf and all ancestors and to rate bucket for ancestors at levels
647  * "level" and higher. It also handles possible change of mode resulting
648  * from the update. Note that mode can also increase here (MAY_BORROW to
649  * CAN_SEND) because we can use more precise clock that event queue here.
650  * In such case we remove class from event queue first.
651  */
652 static void htb_charge_class(struct htb_sched *q, struct htb_class *cl,
653                              int level, struct sk_buff *skb)
654 {
655         int bytes = skb->len;
656         long toks, diff;
657         enum htb_cmode old_mode;
658
659 #define HTB_ACCNT(T,B,R) toks = diff + cl->T; \
660         if (toks > cl->B) toks = cl->B; \
661         toks -= L2T(cl, cl->R, bytes); \
662         if (toks <= -cl->mbuffer) toks = 1-cl->mbuffer; \
663         cl->T = toks
664
665         while (cl) {
666                 diff = psched_tdiff_bounded(q->now, cl->t_c, cl->mbuffer);
667                 if (cl->level >= level) {
668                         if (cl->level == level)
669                                 cl->xstats.lends++;
670                         HTB_ACCNT(tokens, buffer, rate);
671                 } else {
672                         cl->xstats.borrows++;
673                         cl->tokens += diff;     /* we moved t_c; update tokens */
674                 }
675                 HTB_ACCNT(ctokens, cbuffer, ceil);
676                 cl->t_c = q->now;
677
678                 old_mode = cl->cmode;
679                 diff = 0;
680                 htb_change_class_mode(q, cl, &diff);
681                 if (old_mode != cl->cmode) {
682                         if (old_mode != HTB_CAN_SEND)
683                                 htb_safe_rb_erase(&cl->pq_node, q->wait_pq + cl->level);
684                         if (cl->cmode != HTB_CAN_SEND)
685                                 htb_add_to_wait_tree(q, cl, diff);
686                 }
687
688                 /* update byte stats except for leaves which are already updated */
689                 if (cl->level) {
690                         cl->bstats.bytes += bytes;
691                         cl->bstats.packets += skb_is_gso(skb)?
692                                         skb_shinfo(skb)->gso_segs:1;
693                 }
694                 cl = cl->parent;
695         }
696 }
697
698 /**
699  * htb_do_events - make mode changes to classes at the level
700  *
701  * Scans event queue for pending events and applies them. Returns time of
702  * next pending event (0 for no event in pq).
703  * Note: Applied are events whose have cl->pq_key <= q->now.
704  */
705 static psched_time_t htb_do_events(struct htb_sched *q, int level)
706 {
707         int i;
708
709         for (i = 0; i < 500; i++) {
710                 struct htb_class *cl;
711                 long diff;
712                 struct rb_node *p = rb_first(&q->wait_pq[level]);
713
714                 if (!p)
715                         return 0;
716
717                 cl = rb_entry(p, struct htb_class, pq_node);
718                 if (cl->pq_key > q->now)
719                         return cl->pq_key;
720
721                 htb_safe_rb_erase(p, q->wait_pq + level);
722                 diff = psched_tdiff_bounded(q->now, cl->t_c, cl->mbuffer);
723                 htb_change_class_mode(q, cl, &diff);
724                 if (cl->cmode != HTB_CAN_SEND)
725                         htb_add_to_wait_tree(q, cl, diff);
726         }
727         if (net_ratelimit())
728                 printk(KERN_WARNING "htb: too many events !\n");
729         return q->now + PSCHED_TICKS_PER_SEC / 10;
730 }
731
732 /* Returns class->node+prio from id-tree where classe's id is >= id. NULL
733    is no such one exists. */
734 static struct rb_node *htb_id_find_next_upper(int prio, struct rb_node *n,
735                                               u32 id)
736 {
737         struct rb_node *r = NULL;
738         while (n) {
739                 struct htb_class *cl =
740                     rb_entry(n, struct htb_class, node[prio]);
741                 if (id == cl->classid)
742                         return n;
743
744                 if (id > cl->classid) {
745                         n = n->rb_right;
746                 } else {
747                         r = n;
748                         n = n->rb_left;
749                 }
750         }
751         return r;
752 }
753
754 /**
755  * htb_lookup_leaf - returns next leaf class in DRR order
756  *
757  * Find leaf where current feed pointers points to.
758  */
759 static struct htb_class *htb_lookup_leaf(struct rb_root *tree, int prio,
760                                          struct rb_node **pptr, u32 * pid)
761 {
762         int i;
763         struct {
764                 struct rb_node *root;
765                 struct rb_node **pptr;
766                 u32 *pid;
767         } stk[TC_HTB_MAXDEPTH], *sp = stk;
768
769         BUG_TRAP(tree->rb_node);
770         sp->root = tree->rb_node;
771         sp->pptr = pptr;
772         sp->pid = pid;
773
774         for (i = 0; i < 65535; i++) {
775                 if (!*sp->pptr && *sp->pid) {
776                         /* ptr was invalidated but id is valid - try to recover
777                            the original or next ptr */
778                         *sp->pptr =
779                             htb_id_find_next_upper(prio, sp->root, *sp->pid);
780                 }
781                 *sp->pid = 0;   /* ptr is valid now so that remove this hint as it
782                                    can become out of date quickly */
783                 if (!*sp->pptr) {       /* we are at right end; rewind & go up */
784                         *sp->pptr = sp->root;
785                         while ((*sp->pptr)->rb_left)
786                                 *sp->pptr = (*sp->pptr)->rb_left;
787                         if (sp > stk) {
788                                 sp--;
789                                 BUG_TRAP(*sp->pptr);
790                                 if (!*sp->pptr)
791                                         return NULL;
792                                 htb_next_rb_node(sp->pptr);
793                         }
794                 } else {
795                         struct htb_class *cl;
796                         cl = rb_entry(*sp->pptr, struct htb_class, node[prio]);
797                         if (!cl->level)
798                                 return cl;
799                         (++sp)->root = cl->un.inner.feed[prio].rb_node;
800                         sp->pptr = cl->un.inner.ptr + prio;
801                         sp->pid = cl->un.inner.last_ptr_id + prio;
802                 }
803         }
804         BUG_TRAP(0);
805         return NULL;
806 }
807
808 /* dequeues packet at given priority and level; call only if
809    you are sure that there is active class at prio/level */
810 static struct sk_buff *htb_dequeue_tree(struct htb_sched *q, int prio,
811                                         int level)
812 {
813         struct sk_buff *skb = NULL;
814         struct htb_class *cl, *start;
815         /* look initial class up in the row */
816         start = cl = htb_lookup_leaf(q->row[level] + prio, prio,
817                                      q->ptr[level] + prio,
818                                      q->last_ptr_id[level] + prio);
819
820         do {
821 next:
822                 BUG_TRAP(cl);
823                 if (!cl)
824                         return NULL;
825
826                 /* class can be empty - it is unlikely but can be true if leaf
827                    qdisc drops packets in enqueue routine or if someone used
828                    graft operation on the leaf since last dequeue;
829                    simply deactivate and skip such class */
830                 if (unlikely(cl->un.leaf.q->q.qlen == 0)) {
831                         struct htb_class *next;
832                         htb_deactivate(q, cl);
833
834                         /* row/level might become empty */
835                         if ((q->row_mask[level] & (1 << prio)) == 0)
836                                 return NULL;
837
838                         next = htb_lookup_leaf(q->row[level] + prio,
839                                                prio, q->ptr[level] + prio,
840                                                q->last_ptr_id[level] + prio);
841
842                         if (cl == start)        /* fix start if we just deleted it */
843                                 start = next;
844                         cl = next;
845                         goto next;
846                 }
847
848                 skb = cl->un.leaf.q->dequeue(cl->un.leaf.q);
849                 if (likely(skb != NULL))
850                         break;
851                 if (!cl->warned) {
852                         printk(KERN_WARNING
853                                "htb: class %X isn't work conserving ?!\n",
854                                cl->classid);
855                         cl->warned = 1;
856                 }
857                 q->nwc_hit++;
858                 htb_next_rb_node((level ? cl->parent->un.inner.ptr : q->
859                                   ptr[0]) + prio);
860                 cl = htb_lookup_leaf(q->row[level] + prio, prio,
861                                      q->ptr[level] + prio,
862                                      q->last_ptr_id[level] + prio);
863
864         } while (cl != start);
865
866         if (likely(skb != NULL)) {
867                 if ((cl->un.leaf.deficit[level] -= skb->len) < 0) {
868                         cl->un.leaf.deficit[level] += cl->un.leaf.quantum;
869                         htb_next_rb_node((level ? cl->parent->un.inner.ptr : q->
870                                           ptr[0]) + prio);
871                 }
872                 /* this used to be after charge_class but this constelation
873                    gives us slightly better performance */
874                 if (!cl->un.leaf.q->q.qlen)
875                         htb_deactivate(q, cl);
876                 htb_charge_class(q, cl, level, skb);
877         }
878         return skb;
879 }
880
881 static struct sk_buff *htb_dequeue(struct Qdisc *sch)
882 {
883         struct sk_buff *skb = NULL;
884         struct htb_sched *q = qdisc_priv(sch);
885         int level;
886         psched_time_t next_event;
887
888         /* try to dequeue direct packets as high prio (!) to minimize cpu work */
889         skb = __skb_dequeue(&q->direct_queue);
890         if (skb != NULL) {
891                 sch->flags &= ~TCQ_F_THROTTLED;
892                 sch->q.qlen--;
893                 return skb;
894         }
895
896         if (!sch->q.qlen)
897                 goto fin;
898         q->now = psched_get_time();
899
900         next_event = q->now + 5 * PSCHED_TICKS_PER_SEC;
901         q->nwc_hit = 0;
902         for (level = 0; level < TC_HTB_MAXDEPTH; level++) {
903                 /* common case optimization - skip event handler quickly */
904                 int m;
905                 psched_time_t event;
906
907                 if (q->now >= q->near_ev_cache[level]) {
908                         event = htb_do_events(q, level);
909                         if (!event)
910                                 event = q->now + PSCHED_TICKS_PER_SEC;
911                         q->near_ev_cache[level] = event;
912                 } else
913                         event = q->near_ev_cache[level];
914
915                 if (event && next_event > event)
916                         next_event = event;
917
918                 m = ~q->row_mask[level];
919                 while (m != (int)(-1)) {
920                         int prio = ffz(m);
921                         m |= 1 << prio;
922                         skb = htb_dequeue_tree(q, prio, level);
923                         if (likely(skb != NULL)) {
924                                 sch->q.qlen--;
925                                 sch->flags &= ~TCQ_F_THROTTLED;
926                                 goto fin;
927                         }
928                 }
929         }
930         sch->qstats.overlimits++;
931         qdisc_watchdog_schedule(&q->watchdog, next_event);
932 fin:
933         return skb;
934 }
935
936 /* try to drop from each class (by prio) until one succeed */
937 static unsigned int htb_drop(struct Qdisc *sch)
938 {
939         struct htb_sched *q = qdisc_priv(sch);
940         int prio;
941
942         for (prio = TC_HTB_NUMPRIO - 1; prio >= 0; prio--) {
943                 struct list_head *p;
944                 list_for_each(p, q->drops + prio) {
945                         struct htb_class *cl = list_entry(p, struct htb_class,
946                                                           un.leaf.drop_list);
947                         unsigned int len;
948                         if (cl->un.leaf.q->ops->drop &&
949                             (len = cl->un.leaf.q->ops->drop(cl->un.leaf.q))) {
950                                 sch->q.qlen--;
951                                 if (!cl->un.leaf.q->q.qlen)
952                                         htb_deactivate(q, cl);
953                                 return len;
954                         }
955                 }
956         }
957         return 0;
958 }
959
960 /* reset all classes */
961 /* always caled under BH & queue lock */
962 static void htb_reset(struct Qdisc *sch)
963 {
964         struct htb_sched *q = qdisc_priv(sch);
965         int i;
966
967         for (i = 0; i < HTB_HSIZE; i++) {
968                 struct hlist_node *p;
969                 struct htb_class *cl;
970
971                 hlist_for_each_entry(cl, p, q->hash + i, hlist) {
972                         if (cl->level)
973                                 memset(&cl->un.inner, 0, sizeof(cl->un.inner));
974                         else {
975                                 if (cl->un.leaf.q)
976                                         qdisc_reset(cl->un.leaf.q);
977                                 INIT_LIST_HEAD(&cl->un.leaf.drop_list);
978                         }
979                         cl->prio_activity = 0;
980                         cl->cmode = HTB_CAN_SEND;
981
982                 }
983         }
984         qdisc_watchdog_cancel(&q->watchdog);
985         __skb_queue_purge(&q->direct_queue);
986         sch->q.qlen = 0;
987         memset(q->row, 0, sizeof(q->row));
988         memset(q->row_mask, 0, sizeof(q->row_mask));
989         memset(q->wait_pq, 0, sizeof(q->wait_pq));
990         memset(q->ptr, 0, sizeof(q->ptr));
991         for (i = 0; i < TC_HTB_NUMPRIO; i++)
992                 INIT_LIST_HEAD(q->drops + i);
993 }
994
995 static int htb_init(struct Qdisc *sch, struct nlattr *opt)
996 {
997         struct htb_sched *q = qdisc_priv(sch);
998         struct nlattr *tb[TCA_HTB_INIT + 1];
999         struct tc_htb_glob *gopt;
1000         int err;
1001         int i;
1002
1003         if (!opt)
1004                 return -EINVAL;
1005
1006         err = nla_parse_nested(tb, TCA_HTB_INIT, opt, NULL);
1007         if (err < 0)
1008                 return err;
1009
1010         if (tb[TCA_HTB_INIT] == NULL ||
1011             nla_len(tb[TCA_HTB_INIT]) < sizeof(*gopt)) {
1012                 printk(KERN_ERR "HTB: hey probably you have bad tc tool ?\n");
1013                 return -EINVAL;
1014         }
1015         gopt = nla_data(tb[TCA_HTB_INIT]);
1016         if (gopt->version != HTB_VER >> 16) {
1017                 printk(KERN_ERR
1018                        "HTB: need tc/htb version %d (minor is %d), you have %d\n",
1019                        HTB_VER >> 16, HTB_VER & 0xffff, gopt->version);
1020                 return -EINVAL;
1021         }
1022
1023         INIT_LIST_HEAD(&q->root);
1024         for (i = 0; i < HTB_HSIZE; i++)
1025                 INIT_HLIST_HEAD(q->hash + i);
1026         for (i = 0; i < TC_HTB_NUMPRIO; i++)
1027                 INIT_LIST_HEAD(q->drops + i);
1028
1029         qdisc_watchdog_init(&q->watchdog, sch);
1030         skb_queue_head_init(&q->direct_queue);
1031
1032         q->direct_qlen = sch->dev->tx_queue_len;
1033         if (q->direct_qlen < 2) /* some devices have zero tx_queue_len */
1034                 q->direct_qlen = 2;
1035
1036         if ((q->rate2quantum = gopt->rate2quantum) < 1)
1037                 q->rate2quantum = 1;
1038         q->defcls = gopt->defcls;
1039
1040         return 0;
1041 }
1042
1043 static int htb_dump(struct Qdisc *sch, struct sk_buff *skb)
1044 {
1045         struct htb_sched *q = qdisc_priv(sch);
1046         unsigned char *b = skb_tail_pointer(skb);
1047         struct nlattr *nla;
1048         struct tc_htb_glob gopt;
1049         spin_lock_bh(&sch->dev->queue_lock);
1050         gopt.direct_pkts = q->direct_pkts;
1051
1052         gopt.version = HTB_VER;
1053         gopt.rate2quantum = q->rate2quantum;
1054         gopt.defcls = q->defcls;
1055         gopt.debug = 0;
1056         nla = (struct nlattr *)b;
1057         NLA_PUT(skb, TCA_OPTIONS, 0, NULL);
1058         NLA_PUT(skb, TCA_HTB_INIT, sizeof(gopt), &gopt);
1059         nla->nla_len = skb_tail_pointer(skb) - b;
1060         spin_unlock_bh(&sch->dev->queue_lock);
1061         return skb->len;
1062 nla_put_failure:
1063         spin_unlock_bh(&sch->dev->queue_lock);
1064         nlmsg_trim(skb, skb_tail_pointer(skb));
1065         return -1;
1066 }
1067
1068 static int htb_dump_class(struct Qdisc *sch, unsigned long arg,
1069                           struct sk_buff *skb, struct tcmsg *tcm)
1070 {
1071         struct htb_class *cl = (struct htb_class *)arg;
1072         unsigned char *b = skb_tail_pointer(skb);
1073         struct nlattr *nla;
1074         struct tc_htb_opt opt;
1075
1076         spin_lock_bh(&sch->dev->queue_lock);
1077         tcm->tcm_parent = cl->parent ? cl->parent->classid : TC_H_ROOT;
1078         tcm->tcm_handle = cl->classid;
1079         if (!cl->level && cl->un.leaf.q)
1080                 tcm->tcm_info = cl->un.leaf.q->handle;
1081
1082         nla = (struct nlattr *)b;
1083         NLA_PUT(skb, TCA_OPTIONS, 0, NULL);
1084
1085         memset(&opt, 0, sizeof(opt));
1086
1087         opt.rate = cl->rate->rate;
1088         opt.buffer = cl->buffer;
1089         opt.ceil = cl->ceil->rate;
1090         opt.cbuffer = cl->cbuffer;
1091         opt.quantum = cl->un.leaf.quantum;
1092         opt.prio = cl->un.leaf.prio;
1093         opt.level = cl->level;
1094         NLA_PUT(skb, TCA_HTB_PARMS, sizeof(opt), &opt);
1095         nla->nla_len = skb_tail_pointer(skb) - b;
1096         spin_unlock_bh(&sch->dev->queue_lock);
1097         return skb->len;
1098 nla_put_failure:
1099         spin_unlock_bh(&sch->dev->queue_lock);
1100         nlmsg_trim(skb, b);
1101         return -1;
1102 }
1103
1104 static int
1105 htb_dump_class_stats(struct Qdisc *sch, unsigned long arg, struct gnet_dump *d)
1106 {
1107         struct htb_class *cl = (struct htb_class *)arg;
1108
1109         if (!cl->level && cl->un.leaf.q)
1110                 cl->qstats.qlen = cl->un.leaf.q->q.qlen;
1111         cl->xstats.tokens = cl->tokens;
1112         cl->xstats.ctokens = cl->ctokens;
1113
1114         if (gnet_stats_copy_basic(d, &cl->bstats) < 0 ||
1115             gnet_stats_copy_rate_est(d, &cl->rate_est) < 0 ||
1116             gnet_stats_copy_queue(d, &cl->qstats) < 0)
1117                 return -1;
1118
1119         return gnet_stats_copy_app(d, &cl->xstats, sizeof(cl->xstats));
1120 }
1121
1122 static int htb_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
1123                      struct Qdisc **old)
1124 {
1125         struct htb_class *cl = (struct htb_class *)arg;
1126
1127         if (cl && !cl->level) {
1128                 if (new == NULL &&
1129                     (new = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops,
1130                                              cl->classid))
1131                     == NULL)
1132                         return -ENOBUFS;
1133                 sch_tree_lock(sch);
1134                 if ((*old = xchg(&cl->un.leaf.q, new)) != NULL) {
1135                         qdisc_tree_decrease_qlen(*old, (*old)->q.qlen);
1136                         qdisc_reset(*old);
1137                 }
1138                 sch_tree_unlock(sch);
1139                 return 0;
1140         }
1141         return -ENOENT;
1142 }
1143
1144 static struct Qdisc *htb_leaf(struct Qdisc *sch, unsigned long arg)
1145 {
1146         struct htb_class *cl = (struct htb_class *)arg;
1147         return (cl && !cl->level) ? cl->un.leaf.q : NULL;
1148 }
1149
1150 static void htb_qlen_notify(struct Qdisc *sch, unsigned long arg)
1151 {
1152         struct htb_class *cl = (struct htb_class *)arg;
1153
1154         if (cl->un.leaf.q->q.qlen == 0)
1155                 htb_deactivate(qdisc_priv(sch), cl);
1156 }
1157
1158 static unsigned long htb_get(struct Qdisc *sch, u32 classid)
1159 {
1160         struct htb_class *cl = htb_find(classid, sch);
1161         if (cl)
1162                 cl->refcnt++;
1163         return (unsigned long)cl;
1164 }
1165
1166 static inline int htb_parent_last_child(struct htb_class *cl)
1167 {
1168         if (!cl->parent)
1169                 /* the root class */
1170                 return 0;
1171
1172         if (!(cl->parent->children.next == &cl->sibling &&
1173                 cl->parent->children.prev == &cl->sibling))
1174                 /* not the last child */
1175                 return 0;
1176
1177         return 1;
1178 }
1179
1180 static void htb_parent_to_leaf(struct htb_class *cl, struct Qdisc *new_q)
1181 {
1182         struct htb_class *parent = cl->parent;
1183
1184         BUG_TRAP(!cl->level && cl->un.leaf.q && !cl->prio_activity);
1185
1186         parent->level = 0;
1187         memset(&parent->un.inner, 0, sizeof(parent->un.inner));
1188         INIT_LIST_HEAD(&parent->un.leaf.drop_list);
1189         parent->un.leaf.q = new_q ? new_q : &noop_qdisc;
1190         parent->un.leaf.quantum = parent->quantum;
1191         parent->un.leaf.prio = parent->prio;
1192         parent->tokens = parent->buffer;
1193         parent->ctokens = parent->cbuffer;
1194         parent->t_c = psched_get_time();
1195         parent->cmode = HTB_CAN_SEND;
1196 }
1197
1198 static void htb_destroy_class(struct Qdisc *sch, struct htb_class *cl)
1199 {
1200         struct htb_sched *q = qdisc_priv(sch);
1201
1202         if (!cl->level) {
1203                 BUG_TRAP(cl->un.leaf.q);
1204                 qdisc_destroy(cl->un.leaf.q);
1205         }
1206         gen_kill_estimator(&cl->bstats, &cl->rate_est);
1207         qdisc_put_rtab(cl->rate);
1208         qdisc_put_rtab(cl->ceil);
1209
1210         tcf_destroy_chain(cl->filter_list);
1211
1212         while (!list_empty(&cl->children))
1213                 htb_destroy_class(sch, list_entry(cl->children.next,
1214                                                   struct htb_class, sibling));
1215
1216         /* note: this delete may happen twice (see htb_delete) */
1217         hlist_del_init(&cl->hlist);
1218         list_del(&cl->sibling);
1219
1220         if (cl->prio_activity)
1221                 htb_deactivate(q, cl);
1222
1223         if (cl->cmode != HTB_CAN_SEND)
1224                 htb_safe_rb_erase(&cl->pq_node, q->wait_pq + cl->level);
1225
1226         kfree(cl);
1227 }
1228
1229 /* always caled under BH & queue lock */
1230 static void htb_destroy(struct Qdisc *sch)
1231 {
1232         struct htb_sched *q = qdisc_priv(sch);
1233
1234         qdisc_watchdog_cancel(&q->watchdog);
1235         /* This line used to be after htb_destroy_class call below
1236            and surprisingly it worked in 2.4. But it must precede it
1237            because filter need its target class alive to be able to call
1238            unbind_filter on it (without Oops). */
1239         tcf_destroy_chain(q->filter_list);
1240
1241         while (!list_empty(&q->root))
1242                 htb_destroy_class(sch, list_entry(q->root.next,
1243                                                   struct htb_class, sibling));
1244
1245         __skb_queue_purge(&q->direct_queue);
1246 }
1247
1248 static int htb_delete(struct Qdisc *sch, unsigned long arg)
1249 {
1250         struct htb_sched *q = qdisc_priv(sch);
1251         struct htb_class *cl = (struct htb_class *)arg;
1252         unsigned int qlen;
1253         struct Qdisc *new_q = NULL;
1254         int last_child = 0;
1255
1256         // TODO: why don't allow to delete subtree ? references ? does
1257         // tc subsys quarantee us that in htb_destroy it holds no class
1258         // refs so that we can remove children safely there ?
1259         if (!list_empty(&cl->children) || cl->filter_cnt)
1260                 return -EBUSY;
1261
1262         if (!cl->level && htb_parent_last_child(cl)) {
1263                 new_q = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops,
1264                                                 cl->parent->classid);
1265                 last_child = 1;
1266         }
1267
1268         sch_tree_lock(sch);
1269
1270         if (!cl->level) {
1271                 qlen = cl->un.leaf.q->q.qlen;
1272                 qdisc_reset(cl->un.leaf.q);
1273                 qdisc_tree_decrease_qlen(cl->un.leaf.q, qlen);
1274         }
1275
1276         /* delete from hash and active; remainder in destroy_class */
1277         hlist_del_init(&cl->hlist);
1278
1279         if (cl->prio_activity)
1280                 htb_deactivate(q, cl);
1281
1282         if (last_child)
1283                 htb_parent_to_leaf(cl, new_q);
1284
1285         if (--cl->refcnt == 0)
1286                 htb_destroy_class(sch, cl);
1287
1288         sch_tree_unlock(sch);
1289         return 0;
1290 }
1291
1292 static void htb_put(struct Qdisc *sch, unsigned long arg)
1293 {
1294         struct htb_class *cl = (struct htb_class *)arg;
1295
1296         if (--cl->refcnt == 0)
1297                 htb_destroy_class(sch, cl);
1298 }
1299
1300 static int htb_change_class(struct Qdisc *sch, u32 classid,
1301                             u32 parentid, struct nlattr **tca,
1302                             unsigned long *arg)
1303 {
1304         int err = -EINVAL;
1305         struct htb_sched *q = qdisc_priv(sch);
1306         struct htb_class *cl = (struct htb_class *)*arg, *parent;
1307         struct nlattr *opt = tca[TCA_OPTIONS];
1308         struct qdisc_rate_table *rtab = NULL, *ctab = NULL;
1309         struct nlattr *tb[TCA_HTB_RTAB + 1];
1310         struct tc_htb_opt *hopt;
1311
1312         /* extract all subattrs from opt attr */
1313         if (!opt)
1314                 goto failure;
1315
1316         err = nla_parse_nested(tb, TCA_HTB_RTAB, opt, NULL);
1317         if (err < 0)
1318                 goto failure;
1319
1320         err = -EINVAL;
1321         if (tb[TCA_HTB_PARMS] == NULL ||
1322             nla_len(tb[TCA_HTB_PARMS]) < sizeof(*hopt))
1323                 goto failure;
1324
1325         parent = parentid == TC_H_ROOT ? NULL : htb_find(parentid, sch);
1326
1327         hopt = nla_data(tb[TCA_HTB_PARMS]);
1328
1329         rtab = qdisc_get_rtab(&hopt->rate, tb[TCA_HTB_RTAB]);
1330         ctab = qdisc_get_rtab(&hopt->ceil, tb[TCA_HTB_CTAB]);
1331         if (!rtab || !ctab)
1332                 goto failure;
1333
1334         if (!cl) {              /* new class */
1335                 struct Qdisc *new_q;
1336                 int prio;
1337                 struct {
1338                         struct nlattr           nla;
1339                         struct gnet_estimator   opt;
1340                 } est = {
1341                         .nla = {
1342                                 .nla_len        = nla_attr_size(sizeof(est.opt)),
1343                                 .nla_type       = TCA_RATE,
1344                         },
1345                         .opt = {
1346                                 /* 4s interval, 16s averaging constant */
1347                                 .interval       = 2,
1348                                 .ewma_log       = 2,
1349                         },
1350                 };
1351
1352                 /* check for valid classid */
1353                 if (!classid || TC_H_MAJ(classid ^ sch->handle)
1354                     || htb_find(classid, sch))
1355                         goto failure;
1356
1357                 /* check maximal depth */
1358                 if (parent && parent->parent && parent->parent->level < 2) {
1359                         printk(KERN_ERR "htb: tree is too deep\n");
1360                         goto failure;
1361                 }
1362                 err = -ENOBUFS;
1363                 if ((cl = kzalloc(sizeof(*cl), GFP_KERNEL)) == NULL)
1364                         goto failure;
1365
1366                 gen_new_estimator(&cl->bstats, &cl->rate_est,
1367                                   &sch->dev->queue_lock,
1368                                   tca[TCA_RATE] ? : &est.nla);
1369                 cl->refcnt = 1;
1370                 INIT_LIST_HEAD(&cl->sibling);
1371                 INIT_HLIST_NODE(&cl->hlist);
1372                 INIT_LIST_HEAD(&cl->children);
1373                 INIT_LIST_HEAD(&cl->un.leaf.drop_list);
1374                 RB_CLEAR_NODE(&cl->pq_node);
1375
1376                 for (prio = 0; prio < TC_HTB_NUMPRIO; prio++)
1377                         RB_CLEAR_NODE(&cl->node[prio]);
1378
1379                 /* create leaf qdisc early because it uses kmalloc(GFP_KERNEL)
1380                    so that can't be used inside of sch_tree_lock
1381                    -- thanks to Karlis Peisenieks */
1382                 new_q = qdisc_create_dflt(sch->dev, &pfifo_qdisc_ops, classid);
1383                 sch_tree_lock(sch);
1384                 if (parent && !parent->level) {
1385                         unsigned int qlen = parent->un.leaf.q->q.qlen;
1386
1387                         /* turn parent into inner node */
1388                         qdisc_reset(parent->un.leaf.q);
1389                         qdisc_tree_decrease_qlen(parent->un.leaf.q, qlen);
1390                         qdisc_destroy(parent->un.leaf.q);
1391                         if (parent->prio_activity)
1392                                 htb_deactivate(q, parent);
1393
1394                         /* remove from evt list because of level change */
1395                         if (parent->cmode != HTB_CAN_SEND) {
1396                                 htb_safe_rb_erase(&parent->pq_node, q->wait_pq);
1397                                 parent->cmode = HTB_CAN_SEND;
1398                         }
1399                         parent->level = (parent->parent ? parent->parent->level
1400                                          : TC_HTB_MAXDEPTH) - 1;
1401                         memset(&parent->un.inner, 0, sizeof(parent->un.inner));
1402                 }
1403                 /* leaf (we) needs elementary qdisc */
1404                 cl->un.leaf.q = new_q ? new_q : &noop_qdisc;
1405
1406                 cl->classid = classid;
1407                 cl->parent = parent;
1408
1409                 /* set class to be in HTB_CAN_SEND state */
1410                 cl->tokens = hopt->buffer;
1411                 cl->ctokens = hopt->cbuffer;
1412                 cl->mbuffer = 60 * PSCHED_TICKS_PER_SEC;        /* 1min */
1413                 cl->t_c = psched_get_time();
1414                 cl->cmode = HTB_CAN_SEND;
1415
1416                 /* attach to the hash list and parent's family */
1417                 hlist_add_head(&cl->hlist, q->hash + htb_hash(classid));
1418                 list_add_tail(&cl->sibling,
1419                               parent ? &parent->children : &q->root);
1420         } else {
1421                 if (tca[TCA_RATE])
1422                         gen_replace_estimator(&cl->bstats, &cl->rate_est,
1423                                               &sch->dev->queue_lock,
1424                                               tca[TCA_RATE]);
1425                 sch_tree_lock(sch);
1426         }
1427
1428         /* it used to be a nasty bug here, we have to check that node
1429            is really leaf before changing cl->un.leaf ! */
1430         if (!cl->level) {
1431                 cl->un.leaf.quantum = rtab->rate.rate / q->rate2quantum;
1432                 if (!hopt->quantum && cl->un.leaf.quantum < 1000) {
1433                         printk(KERN_WARNING
1434                                "HTB: quantum of class %X is small. Consider r2q change.\n",
1435                                cl->classid);
1436                         cl->un.leaf.quantum = 1000;
1437                 }
1438                 if (!hopt->quantum && cl->un.leaf.quantum > 200000) {
1439                         printk(KERN_WARNING
1440                                "HTB: quantum of class %X is big. Consider r2q change.\n",
1441                                cl->classid);
1442                         cl->un.leaf.quantum = 200000;
1443                 }
1444                 if (hopt->quantum)
1445                         cl->un.leaf.quantum = hopt->quantum;
1446                 if ((cl->un.leaf.prio = hopt->prio) >= TC_HTB_NUMPRIO)
1447                         cl->un.leaf.prio = TC_HTB_NUMPRIO - 1;
1448
1449                 /* backup for htb_parent_to_leaf */
1450                 cl->quantum = cl->un.leaf.quantum;
1451                 cl->prio = cl->un.leaf.prio;
1452         }
1453
1454         cl->buffer = hopt->buffer;
1455         cl->cbuffer = hopt->cbuffer;
1456         if (cl->rate)
1457                 qdisc_put_rtab(cl->rate);
1458         cl->rate = rtab;
1459         if (cl->ceil)
1460                 qdisc_put_rtab(cl->ceil);
1461         cl->ceil = ctab;
1462         sch_tree_unlock(sch);
1463
1464         *arg = (unsigned long)cl;
1465         return 0;
1466
1467 failure:
1468         if (rtab)
1469                 qdisc_put_rtab(rtab);
1470         if (ctab)
1471                 qdisc_put_rtab(ctab);
1472         return err;
1473 }
1474
1475 static struct tcf_proto **htb_find_tcf(struct Qdisc *sch, unsigned long arg)
1476 {
1477         struct htb_sched *q = qdisc_priv(sch);
1478         struct htb_class *cl = (struct htb_class *)arg;
1479         struct tcf_proto **fl = cl ? &cl->filter_list : &q->filter_list;
1480
1481         return fl;
1482 }
1483
1484 static unsigned long htb_bind_filter(struct Qdisc *sch, unsigned long parent,
1485                                      u32 classid)
1486 {
1487         struct htb_sched *q = qdisc_priv(sch);
1488         struct htb_class *cl = htb_find(classid, sch);
1489
1490         /*if (cl && !cl->level) return 0;
1491            The line above used to be there to prevent attaching filters to
1492            leaves. But at least tc_index filter uses this just to get class
1493            for other reasons so that we have to allow for it.
1494            ----
1495            19.6.2002 As Werner explained it is ok - bind filter is just
1496            another way to "lock" the class - unlike "get" this lock can
1497            be broken by class during destroy IIUC.
1498          */
1499         if (cl)
1500                 cl->filter_cnt++;
1501         else
1502                 q->filter_cnt++;
1503         return (unsigned long)cl;
1504 }
1505
1506 static void htb_unbind_filter(struct Qdisc *sch, unsigned long arg)
1507 {
1508         struct htb_sched *q = qdisc_priv(sch);
1509         struct htb_class *cl = (struct htb_class *)arg;
1510
1511         if (cl)
1512                 cl->filter_cnt--;
1513         else
1514                 q->filter_cnt--;
1515 }
1516
1517 static void htb_walk(struct Qdisc *sch, struct qdisc_walker *arg)
1518 {
1519         struct htb_sched *q = qdisc_priv(sch);
1520         int i;
1521
1522         if (arg->stop)
1523                 return;
1524
1525         for (i = 0; i < HTB_HSIZE; i++) {
1526                 struct hlist_node *p;
1527                 struct htb_class *cl;
1528
1529                 hlist_for_each_entry(cl, p, q->hash + i, hlist) {
1530                         if (arg->count < arg->skip) {
1531                                 arg->count++;
1532                                 continue;
1533                         }
1534                         if (arg->fn(sch, (unsigned long)cl, arg) < 0) {
1535                                 arg->stop = 1;
1536                                 return;
1537                         }
1538                         arg->count++;
1539                 }
1540         }
1541 }
1542
1543 static const struct Qdisc_class_ops htb_class_ops = {
1544         .graft          =       htb_graft,
1545         .leaf           =       htb_leaf,
1546         .qlen_notify    =       htb_qlen_notify,
1547         .get            =       htb_get,
1548         .put            =       htb_put,
1549         .change         =       htb_change_class,
1550         .delete         =       htb_delete,
1551         .walk           =       htb_walk,
1552         .tcf_chain      =       htb_find_tcf,
1553         .bind_tcf       =       htb_bind_filter,
1554         .unbind_tcf     =       htb_unbind_filter,
1555         .dump           =       htb_dump_class,
1556         .dump_stats     =       htb_dump_class_stats,
1557 };
1558
1559 static struct Qdisc_ops htb_qdisc_ops __read_mostly = {
1560         .next           =       NULL,
1561         .cl_ops         =       &htb_class_ops,
1562         .id             =       "htb",
1563         .priv_size      =       sizeof(struct htb_sched),
1564         .enqueue        =       htb_enqueue,
1565         .dequeue        =       htb_dequeue,
1566         .requeue        =       htb_requeue,
1567         .drop           =       htb_drop,
1568         .init           =       htb_init,
1569         .reset          =       htb_reset,
1570         .destroy        =       htb_destroy,
1571         .change         =       NULL /* htb_change */,
1572         .dump           =       htb_dump,
1573         .owner          =       THIS_MODULE,
1574 };
1575
1576 static int __init htb_module_init(void)
1577 {
1578         return register_qdisc(&htb_qdisc_ops);
1579 }
1580 static void __exit htb_module_exit(void)
1581 {
1582         unregister_qdisc(&htb_qdisc_ops);
1583 }
1584
1585 module_init(htb_module_init)
1586 module_exit(htb_module_exit)
1587 MODULE_LICENSE("GPL");