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