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