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