net/sched/sch_sfq.c

   1 /*
   2  * net/sched/sch_sfq.c  Stochastic Fairness Queueing discipline.
   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:     Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
  10  */
  11
  12 #include <linux/module.h>
  13 #include <linux/types.h>
  14 #include <linux/kernel.h>
  15 #include <linux/jiffies.h>
  16 #include <linux/string.h>
  17 #include <linux/in.h>
  18 #include <linux/errno.h>
  19 #include <linux/init.h>
  20 #include <linux/skbuff.h>
  21 #include <linux/siphash.h>
  22 #include <linux/slab.h>
  23 #include <linux/vmalloc.h>
  24 #include <net/netlink.h>
  25 #include <net/pkt_sched.h>
  26 #include <net/pkt_cls.h>
  27 #include <net/red.h>
  28
  29
  30 /*      Stochastic Fairness Queuing algorithm.
  31         =======================================
  32
  33         Source:
  34         Paul E. McKenney "Stochastic Fairness Queuing",
  35         IEEE INFOCOMM'90 Proceedings, San Francisco, 1990.
  36
  37         Paul E. McKenney "Stochastic Fairness Queuing",
  38         "Interworking: Research and Experience", v.2, 1991, p.113-131.
  39
  40
  41         See also:
  42         M. Shreedhar and George Varghese "Efficient Fair
  43         Queuing using Deficit Round Robin", Proc. SIGCOMM 95.
  44
  45
  46         This is not the thing that is usually called (W)FQ nowadays.
  47         It does not use any timestamp mechanism, but instead
  48         processes queues in round-robin order.
  49
  50         ADVANTAGE:
  51
  52         - It is very cheap. Both CPU and memory requirements are minimal.
  53
  54         DRAWBACKS:
  55
  56         - "Stochastic" -> It is not 100% fair.
  57         When hash collisions occur, several flows are considered as one.
  58
  59         - "Round-robin" -> It introduces larger delays than virtual clock
  60         based schemes, and should not be used for isolating interactive
  61         traffic from non-interactive. It means, that this scheduler
  62         should be used as leaf of CBQ or P3, which put interactive traffic
  63         to higher priority band.
  64
  65         We still need true WFQ for top level CSZ, but using WFQ
  66         for the best effort traffic is absolutely pointless:
  67         SFQ is superior for this purpose.
  68
  69         IMPLEMENTATION:
  70         This implementation limits :
  71         - maximal queue length per flow to 127 packets.
  72         - max mtu to 2^18-1;
  73         - max 65408 flows,
  74         - number of hash buckets to 65536.
  75
  76         It is easy to increase these values, but not in flight.  */
  77
  78 #define SFQ_MAX_DEPTH           127 /* max number of packets per flow */
  79 #define SFQ_DEFAULT_FLOWS       128
  80 #define SFQ_MAX_FLOWS           (0x10000 - SFQ_MAX_DEPTH - 1) /* max number of flows */
  81 #define SFQ_EMPTY_SLOT          0xffff
  82 #define SFQ_DEFAULT_HASH_DIVISOR 1024
  83
  84 /* We use 16 bits to store allot, and want to handle packets up to 64K
  85  * Scale allot by 8 (1<<3) so that no overflow occurs.
  86  */
  87 #define SFQ_ALLOT_SHIFT         3
  88 #define SFQ_ALLOT_SIZE(X)       DIV_ROUND_UP(X, 1 << SFQ_ALLOT_SHIFT)
  89
  90 /* This type should contain at least SFQ_MAX_DEPTH + 1 + SFQ_MAX_FLOWS values */
  91 typedef u16 sfq_index;
  92
  93 /*
  94  * We dont use pointers to save space.
  95  * Small indexes [0 ... SFQ_MAX_FLOWS - 1] are 'pointers' to slots[] array
  96  * while following values [SFQ_MAX_FLOWS ... SFQ_MAX_FLOWS + SFQ_MAX_DEPTH]
  97  * are 'pointers' to dep[] array
  98  */
  99 struct sfq_head {
 100         sfq_index       next;
 101         sfq_index       prev;
 102 };
 103
 104 struct sfq_slot {
 105         struct sk_buff  *skblist_next;
 106         struct sk_buff  *skblist_prev;
 107         sfq_index       qlen; /* number of skbs in skblist */
 108         sfq_index       next; /* next slot in sfq RR chain */
 109         struct sfq_head dep; /* anchor in dep[] chains */
 110         unsigned short  hash; /* hash value (index in ht[]) */
 111         short           allot; /* credit for this slot */
 112
 113         unsigned int    backlog;
 114         struct red_vars vars;
 115 };
 116
 117 struct sfq_sched_data {
 118 /* frequently used fields */
 119         int             limit;          /* limit of total number of packets in this qdisc */
 120         unsigned int    divisor;        /* number of slots in hash table */
 121         u8              headdrop;
 122         u8              maxdepth;       /* limit of packets per flow */
 123
 124         siphash_key_t   perturbation;
 125         u8              cur_depth;      /* depth of longest slot */
 126         u8              flags;
 127         unsigned short  scaled_quantum; /* SFQ_ALLOT_SIZE(quantum) */
 128         struct tcf_proto __rcu *filter_list;
 129         struct tcf_block *block;
 130         sfq_index       *ht;            /* Hash table ('divisor' slots) */
 131         struct sfq_slot *slots;         /* Flows table ('maxflows' entries) */
 132
 133         struct red_parms *red_parms;
 134         struct tc_sfqred_stats stats;
 135         struct sfq_slot *tail;          /* current slot in round */
 136
 137         struct sfq_head dep[SFQ_MAX_DEPTH + 1];
 138                                         /* Linked lists of slots, indexed by depth
 139                                          * dep[0] : list of unused flows
 140                                          * dep[1] : list of flows with 1 packet
 141                                          * dep[X] : list of flows with X packets
 142                                          */
 143
 144         unsigned int    maxflows;       /* number of flows in flows array */
 145         int             perturb_period;
 146         unsigned int    quantum;        /* Allotment per round: MUST BE >= MTU */
 147         struct timer_list perturb_timer;
 148         struct Qdisc    *sch;
 149 };
 150
 151 /*
 152  * sfq_head are either in a sfq_slot or in dep[] array
 153  */
 154 static inline struct sfq_head *sfq_dep_head(struct sfq_sched_data *q, sfq_index val)
 155 {
 156         if (val < SFQ_MAX_FLOWS)
 157                 return &q->slots[val].dep;
 158         return &q->dep[val - SFQ_MAX_FLOWS];
 159 }
 160
 161 static unsigned int sfq_hash(const struct sfq_sched_data *q,
 162                              const struct sk_buff *skb)
 163 {
 164         return skb_get_hash_perturb(skb, &q->perturbation) & (q->divisor - 1);
 165 }
 166
 167 static unsigned int sfq_classify(struct sk_buff *skb, struct Qdisc *sch,
 168                                  int *qerr)
 169 {
 170         struct sfq_sched_data *q = qdisc_priv(sch);
 171         struct tcf_result res;
 172         struct tcf_proto *fl;
 173         int result;
 174
 175         if (TC_H_MAJ(skb->priority) == sch->handle &&
 176             TC_H_MIN(skb->priority) > 0 &&
 177             TC_H_MIN(skb->priority) <= q->divisor)
 178                 return TC_H_MIN(skb->priority);
 179
 180         fl = rcu_dereference_bh(q->filter_list);
 181         if (!fl)
 182                 return sfq_hash(q, skb) + 1;
 183
 184         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
 185         result = tcf_classify(skb, fl, &res, false);
 186         if (result >= 0) {
 187 #ifdef CONFIG_NET_CLS_ACT
 188                 switch (result) {
 189                 case TC_ACT_STOLEN:
 190                 case TC_ACT_QUEUED:
 191                 case TC_ACT_TRAP:
 192                         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
 193                         /* fall through */
 194                 case TC_ACT_SHOT:
 195                         return 0;
 196                 }
 197 #endif
 198                 if (TC_H_MIN(res.classid) <= q->divisor)
 199                         return TC_H_MIN(res.classid);
 200         }
 201         return 0;
 202 }
 203
 204 /*
 205  * x : slot number [0 .. SFQ_MAX_FLOWS - 1]
 206  */
 207 static inline void sfq_link(struct sfq_sched_data *q, sfq_index x)
 208 {
 209         sfq_index p, n;
 210         struct sfq_slot *slot = &q->slots[x];
 211         int qlen = slot->qlen;
 212
 213         p = qlen + SFQ_MAX_FLOWS;
 214         n = q->dep[qlen].next;
 215
 216         slot->dep.next = n;
 217         slot->dep.prev = p;
 218
 219         q->dep[qlen].next = x;          /* sfq_dep_head(q, p)->next = x */
 220         sfq_dep_head(q, n)->prev = x;
 221 }
 222
 223 #define sfq_unlink(q, x, n, p)                  \
 224         do {                                    \
 225                 n = q->slots[x].dep.next;       \
 226                 p = q->slots[x].dep.prev;       \
 227                 sfq_dep_head(q, p)->next = n;   \
 228                 sfq_dep_head(q, n)->prev = p;   \
 229         } while (0)
 230
 231
 232 static inline void sfq_dec(struct sfq_sched_data *q, sfq_index x)
 233 {
 234         sfq_index p, n;
 235         int d;
 236
 237         sfq_unlink(q, x, n, p);
 238
 239         d = q->slots[x].qlen--;
 240         if (n == p && q->cur_depth == d)
 241                 q->cur_depth--;
 242         sfq_link(q, x);
 243 }
 244
 245 static inline void sfq_inc(struct sfq_sched_data *q, sfq_index x)
 246 {
 247         sfq_index p, n;
 248         int d;
 249
 250         sfq_unlink(q, x, n, p);
 251
 252         d = ++q->slots[x].qlen;
 253         if (q->cur_depth < d)
 254                 q->cur_depth = d;
 255         sfq_link(q, x);
 256 }
 257
 258 /* helper functions : might be changed when/if skb use a standard list_head */
 259
 260 /* remove one skb from tail of slot queue */
 261 static inline struct sk_buff *slot_dequeue_tail(struct sfq_slot *slot)
 262 {
 263         struct sk_buff *skb = slot->skblist_prev;
 264
 265         slot->skblist_prev = skb->prev;
 266         skb->prev->next = (struct sk_buff *)slot;
 267         skb->next = skb->prev = NULL;
 268         return skb;
 269 }
 270
 271 /* remove one skb from head of slot queue */
 272 static inline struct sk_buff *slot_dequeue_head(struct sfq_slot *slot)
 273 {
 274         struct sk_buff *skb = slot->skblist_next;
 275
 276         slot->skblist_next = skb->next;
 277         skb->next->prev = (struct sk_buff *)slot;
 278         skb->next = skb->prev = NULL;
 279         return skb;
 280 }
 281
 282 static inline void slot_queue_init(struct sfq_slot *slot)
 283 {
 284         memset(slot, 0, sizeof(*slot));
 285         slot->skblist_prev = slot->skblist_next = (struct sk_buff *)slot;
 286 }
 287
 288 /* add skb to slot queue (tail add) */
 289 static inline void slot_queue_add(struct sfq_slot *slot, struct sk_buff *skb)
 290 {
 291         skb->prev = slot->skblist_prev;
 292         skb->next = (struct sk_buff *)slot;
 293         slot->skblist_prev->next = skb;
 294         slot->skblist_prev = skb;
 295 }
 296
 297 static unsigned int sfq_drop(struct Qdisc *sch, struct sk_buff **to_free)
 298 {
 299         struct sfq_sched_data *q = qdisc_priv(sch);
 300         sfq_index x, d = q->cur_depth;
 301         struct sk_buff *skb;
 302         unsigned int len;
 303         struct sfq_slot *slot;
 304
 305         /* Queue is full! Find the longest slot and drop tail packet from it */
 306         if (d > 1) {
 307                 x = q->dep[d].next;
 308                 slot = &q->slots[x];
 309 drop:
 310                 skb = q->headdrop ? slot_dequeue_head(slot) : slot_dequeue_tail(slot);
 311                 len = qdisc_pkt_len(skb);
 312                 slot->backlog -= len;
 313                 sfq_dec(q, x);
 314                 sch->q.qlen--;
 315                 qdisc_qstats_backlog_dec(sch, skb);
 316                 qdisc_drop(skb, sch, to_free);
 317                 return len;
 318         }
 319
 320         if (d == 1) {
 321                 /* It is difficult to believe, but ALL THE SLOTS HAVE LENGTH 1. */
 322                 x = q->tail->next;
 323                 slot = &q->slots[x];
 324                 q->tail->next = slot->next;
 325                 q->ht[slot->hash] = SFQ_EMPTY_SLOT;
 326                 goto drop;
 327         }
 328
 329         return 0;
 330 }
 331
 332 /* Is ECN parameter configured */
 333 static int sfq_prob_mark(const struct sfq_sched_data *q)
 334 {
 335         return q->flags & TC_RED_ECN;
 336 }
 337
 338 /* Should packets over max threshold just be marked */
 339 static int sfq_hard_mark(const struct sfq_sched_data *q)
 340 {
 341         return (q->flags & (TC_RED_ECN | TC_RED_HARDDROP)) == TC_RED_ECN;
 342 }
 343
 344 static int sfq_headdrop(const struct sfq_sched_data *q)
 345 {
 346         return q->headdrop;
 347 }
 348
 349 static int
 350 sfq_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
 351 {
 352         struct sfq_sched_data *q = qdisc_priv(sch);
 353         unsigned int hash, dropped;
 354         sfq_index x, qlen;
 355         struct sfq_slot *slot;
 356         int uninitialized_var(ret);
 357         struct sk_buff *head;
 358         int delta;
 359
 360         hash = sfq_classify(skb, sch, &ret);
 361         if (hash == 0) {
 362                 if (ret & __NET_XMIT_BYPASS)
 363                         qdisc_qstats_drop(sch);
 364                 __qdisc_drop(skb, to_free);
 365                 return ret;
 366         }
 367         hash--;
 368
 369         x = q->ht[hash];
 370         slot = &q->slots[x];
 371         if (x == SFQ_EMPTY_SLOT) {
 372                 x = q->dep[0].next; /* get a free slot */
 373                 if (x >= SFQ_MAX_FLOWS)
 374                         return qdisc_drop(skb, sch, to_free);
 375                 q->ht[hash] = x;
 376                 slot = &q->slots[x];
 377                 slot->hash = hash;
 378                 slot->backlog = 0; /* should already be 0 anyway... */
 379                 red_set_vars(&slot->vars);
 380                 goto enqueue;
 381         }
 382         if (q->red_parms) {
 383                 slot->vars.qavg = red_calc_qavg_no_idle_time(q->red_parms,
 384                                                         &slot->vars,
 385                                                         slot->backlog);
 386                 switch (red_action(q->red_parms,
 387                                    &slot->vars,
 388                                    slot->vars.qavg)) {
 389                 case RED_DONT_MARK:
 390                         break;
 391
 392                 case RED_PROB_MARK:
 393                         qdisc_qstats_overlimit(sch);
 394                         if (sfq_prob_mark(q)) {
 395                                 /* We know we have at least one packet in queue */
 396                                 if (sfq_headdrop(q) &&
 397                                     INET_ECN_set_ce(slot->skblist_next)) {
 398                                         q->stats.prob_mark_head++;
 399                                         break;
 400                                 }
 401                                 if (INET_ECN_set_ce(skb)) {
 402                                         q->stats.prob_mark++;
 403                                         break;
 404                                 }
 405                         }
 406                         q->stats.prob_drop++;
 407                         goto congestion_drop;
 408
 409                 case RED_HARD_MARK:
 410                         qdisc_qstats_overlimit(sch);
 411                         if (sfq_hard_mark(q)) {
 412                                 /* We know we have at least one packet in queue */
 413                                 if (sfq_headdrop(q) &&
 414                                     INET_ECN_set_ce(slot->skblist_next)) {
 415                                         q->stats.forced_mark_head++;
 416                                         break;
 417                                 }
 418                                 if (INET_ECN_set_ce(skb)) {
 419                                         q->stats.forced_mark++;
 420                                         break;
 421                                 }
 422                         }
 423                         q->stats.forced_drop++;
 424                         goto congestion_drop;
 425                 }
 426         }
 427
 428         if (slot->qlen >= q->maxdepth) {
 429 congestion_drop:
 430                 if (!sfq_headdrop(q))
 431                         return qdisc_drop(skb, sch, to_free);
 432
 433                 /* We know we have at least one packet in queue */
 434                 head = slot_dequeue_head(slot);
 435                 delta = qdisc_pkt_len(head) - qdisc_pkt_len(skb);
 436                 sch->qstats.backlog -= delta;
 437                 slot->backlog -= delta;
 438                 qdisc_drop(head, sch, to_free);
 439
 440                 slot_queue_add(slot, skb);
 441                 qdisc_tree_reduce_backlog(sch, 0, delta);
 442                 return NET_XMIT_CN;
 443         }
 444
 445 enqueue:
 446         qdisc_qstats_backlog_inc(sch, skb);
 447         slot->backlog += qdisc_pkt_len(skb);
 448         slot_queue_add(slot, skb);
 449         sfq_inc(q, x);
 450         if (slot->qlen == 1) {          /* The flow is new */
 451                 if (q->tail == NULL) {  /* It is the first flow */
 452                         slot->next = x;
 453                 } else {
 454                         slot->next = q->tail->next;
 455                         q->tail->next = x;
 456                 }
 457                 /* We put this flow at the end of our flow list.
 458                  * This might sound unfair for a new flow to wait after old ones,
 459                  * but we could endup servicing new flows only, and freeze old ones.
 460                  */
 461                 q->tail = slot;
 462                 /* We could use a bigger initial quantum for new flows */
 463                 slot->allot = q->scaled_quantum;
 464         }
 465         if (++sch->q.qlen <= q->limit)
 466                 return NET_XMIT_SUCCESS;
 467
 468         qlen = slot->qlen;
 469         dropped = sfq_drop(sch, to_free);
 470         /* Return Congestion Notification only if we dropped a packet
 471          * from this flow.
 472          */
 473         if (qlen != slot->qlen) {
 474                 qdisc_tree_reduce_backlog(sch, 0, dropped - qdisc_pkt_len(skb));
 475                 return NET_XMIT_CN;
 476         }
 477
 478         /* As we dropped a packet, better let upper stack know this */
 479         qdisc_tree_reduce_backlog(sch, 1, dropped);
 480         return NET_XMIT_SUCCESS;
 481 }
 482
 483 static struct sk_buff *
 484 sfq_dequeue(struct Qdisc *sch)
 485 {
 486         struct sfq_sched_data *q = qdisc_priv(sch);
 487         struct sk_buff *skb;
 488         sfq_index a, next_a;
 489         struct sfq_slot *slot;
 490
 491         /* No active slots */
 492         if (q->tail == NULL)
 493                 return NULL;
 494
 495 next_slot:
 496         a = q->tail->next;
 497         slot = &q->slots[a];
 498         if (slot->allot <= 0) {
 499                 q->tail = slot;
 500                 slot->allot += q->scaled_quantum;
 501                 goto next_slot;
 502         }
 503         skb = slot_dequeue_head(slot);
 504         sfq_dec(q, a);
 505         qdisc_bstats_update(sch, skb);
 506         sch->q.qlen--;
 507         qdisc_qstats_backlog_dec(sch, skb);
 508         slot->backlog -= qdisc_pkt_len(skb);
 509         /* Is the slot empty? */
 510         if (slot->qlen == 0) {
 511                 q->ht[slot->hash] = SFQ_EMPTY_SLOT;
 512                 next_a = slot->next;
 513                 if (a == next_a) {
 514                         q->tail = NULL; /* no more active slots */
 515                         return skb;
 516                 }
 517                 q->tail->next = next_a;
 518         } else {
 519                 slot->allot -= SFQ_ALLOT_SIZE(qdisc_pkt_len(skb));
 520         }
 521         return skb;
 522 }
 523
 524 static void
 525 sfq_reset(struct Qdisc *sch)
 526 {
 527         struct sk_buff *skb;
 528
 529         while ((skb = sfq_dequeue(sch)) != NULL)
 530                 rtnl_kfree_skbs(skb, skb);
 531 }
 532
 533 /*
 534  * When q->perturbation is changed, we rehash all queued skbs
 535  * to avoid OOO (Out Of Order) effects.
 536  * We dont use sfq_dequeue()/sfq_enqueue() because we dont want to change
 537  * counters.
 538  */
 539 static void sfq_rehash(struct Qdisc *sch)
 540 {
 541         struct sfq_sched_data *q = qdisc_priv(sch);
 542         struct sk_buff *skb;
 543         int i;
 544         struct sfq_slot *slot;
 545         struct sk_buff_head list;
 546         int dropped = 0;
 547         unsigned int drop_len = 0;
 548
 549         __skb_queue_head_init(&list);
 550
 551         for (i = 0; i < q->maxflows; i++) {
 552                 slot = &q->slots[i];
 553                 if (!slot->qlen)
 554                         continue;
 555                 while (slot->qlen) {
 556                         skb = slot_dequeue_head(slot);
 557                         sfq_dec(q, i);
 558                         __skb_queue_tail(&list, skb);
 559                 }
 560                 slot->backlog = 0;
 561                 red_set_vars(&slot->vars);
 562                 q->ht[slot->hash] = SFQ_EMPTY_SLOT;
 563         }
 564         q->tail = NULL;
 565
 566         while ((skb = __skb_dequeue(&list)) != NULL) {
 567                 unsigned int hash = sfq_hash(q, skb);
 568                 sfq_index x = q->ht[hash];
 569
 570                 slot = &q->slots[x];
 571                 if (x == SFQ_EMPTY_SLOT) {
 572                         x = q->dep[0].next; /* get a free slot */
 573                         if (x >= SFQ_MAX_FLOWS) {
 574 drop:
 575                                 qdisc_qstats_backlog_dec(sch, skb);
 576                                 drop_len += qdisc_pkt_len(skb);
 577                                 kfree_skb(skb);
 578                                 dropped++;
 579                                 continue;
 580                         }
 581                         q->ht[hash] = x;
 582                         slot = &q->slots[x];
 583                         slot->hash = hash;
 584                 }
 585                 if (slot->qlen >= q->maxdepth)
 586                         goto drop;
 587                 slot_queue_add(slot, skb);
 588                 if (q->red_parms)
 589                         slot->vars.qavg = red_calc_qavg(q->red_parms,
 590                                                         &slot->vars,
 591                                                         slot->backlog);
 592                 slot->backlog += qdisc_pkt_len(skb);
 593                 sfq_inc(q, x);
 594                 if (slot->qlen == 1) {          /* The flow is new */
 595                         if (q->tail == NULL) {  /* It is the first flow */
 596                                 slot->next = x;
 597                         } else {
 598                                 slot->next = q->tail->next;
 599                                 q->tail->next = x;
 600                         }
 601                         q->tail = slot;
 602                         slot->allot = q->scaled_quantum;
 603                 }
 604         }
 605         sch->q.qlen -= dropped;
 606         qdisc_tree_reduce_backlog(sch, dropped, drop_len);
 607 }
 608
 609 static void sfq_perturbation(struct timer_list *t)
 610 {
 611         struct sfq_sched_data *q = from_timer(q, t, perturb_timer);
 612         struct Qdisc *sch = q->sch;
 613         spinlock_t *root_lock = qdisc_lock(qdisc_root_sleeping(sch));
 614         siphash_key_t nkey;
 615
 616         get_random_bytes(&nkey, sizeof(nkey));
 617         spin_lock(root_lock);
 618         q->perturbation = nkey;
 619         if (!q->filter_list && q->tail)
 620                 sfq_rehash(sch);
 621         spin_unlock(root_lock);
 622
 623         if (q->perturb_period)
 624                 mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
 625 }
 626
 627 static int sfq_change(struct Qdisc *sch, struct nlattr *opt)
 628 {
 629         struct sfq_sched_data *q = qdisc_priv(sch);
 630         struct tc_sfq_qopt *ctl = nla_data(opt);
 631         struct tc_sfq_qopt_v1 *ctl_v1 = NULL;
 632         unsigned int qlen, dropped = 0;
 633         struct red_parms *p = NULL;
 634         struct sk_buff *to_free = NULL;
 635         struct sk_buff *tail = NULL;
 636
 637         if (opt->nla_len < nla_attr_size(sizeof(*ctl)))
 638                 return -EINVAL;
 639         if (opt->nla_len >= nla_attr_size(sizeof(*ctl_v1)))
 640                 ctl_v1 = nla_data(opt);
 641         if (ctl->divisor &&
 642             (!is_power_of_2(ctl->divisor) || ctl->divisor > 65536))
 643                 return -EINVAL;
 644
 645         /* slot->allot is a short, make sure quantum is not too big. */
 646         if (ctl->quantum) {
 647                 unsigned int scaled = SFQ_ALLOT_SIZE(ctl->quantum);
 648
 649                 if (scaled <= 0 || scaled > SHRT_MAX)
 650                         return -EINVAL;
 651         }
 652
 653         if (ctl_v1 && !red_check_params(ctl_v1->qth_min, ctl_v1->qth_max,
 654                                         ctl_v1->Wlog, ctl_v1->Scell_log, NULL))
 655                 return -EINVAL;
 656         if (ctl_v1 && ctl_v1->qth_min) {
 657                 p = kmalloc(sizeof(*p), GFP_KERNEL);
 658                 if (!p)
 659                         return -ENOMEM;
 660         }
 661         sch_tree_lock(sch);
 662         if (ctl->quantum) {
 663                 q->quantum = ctl->quantum;
 664                 q->scaled_quantum = SFQ_ALLOT_SIZE(q->quantum);
 665         }
 666         q->perturb_period = ctl->perturb_period * HZ;
 667         if (ctl->flows)
 668                 q->maxflows = min_t(u32, ctl->flows, SFQ_MAX_FLOWS);
 669         if (ctl->divisor) {
 670                 q->divisor = ctl->divisor;
 671                 q->maxflows = min_t(u32, q->maxflows, q->divisor);
 672         }
 673         if (ctl_v1) {
 674                 if (ctl_v1->depth)
 675                         q->maxdepth = min_t(u32, ctl_v1->depth, SFQ_MAX_DEPTH);
 676                 if (p) {
 677                         swap(q->red_parms, p);
 678                         red_set_parms(q->red_parms,
 679                                       ctl_v1->qth_min, ctl_v1->qth_max,
 680                                       ctl_v1->Wlog,
 681                                       ctl_v1->Plog, ctl_v1->Scell_log,
 682                                       NULL,
 683                                       ctl_v1->max_P);
 684                 }
 685                 q->flags = ctl_v1->flags;
 686                 q->headdrop = ctl_v1->headdrop;
 687         }
 688         if (ctl->limit) {
 689                 q->limit = min_t(u32, ctl->limit, q->maxdepth * q->maxflows);
 690                 q->maxflows = min_t(u32, q->maxflows, q->limit);
 691         }
 692
 693         qlen = sch->q.qlen;
 694         while (sch->q.qlen > q->limit) {
 695                 dropped += sfq_drop(sch, &to_free);
 696                 if (!tail)
 697                         tail = to_free;
 698         }
 699
 700         rtnl_kfree_skbs(to_free, tail);
 701         qdisc_tree_reduce_backlog(sch, qlen - sch->q.qlen, dropped);
 702
 703         del_timer(&q->perturb_timer);
 704         if (q->perturb_period) {
 705                 mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
 706                 get_random_bytes(&q->perturbation, sizeof(q->perturbation));
 707         }
 708         sch_tree_unlock(sch);
 709         kfree(p);
 710         return 0;
 711 }
 712
 713 static void *sfq_alloc(size_t sz)
 714 {
 715         return  kvmalloc(sz, GFP_KERNEL);
 716 }
 717
 718 static void sfq_free(void *addr)
 719 {
 720         kvfree(addr);
 721 }
 722
 723 static void sfq_destroy(struct Qdisc *sch)
 724 {
 725         struct sfq_sched_data *q = qdisc_priv(sch);
 726
 727         tcf_block_put(q->block);
 728         q->perturb_period = 0;
 729         del_timer_sync(&q->perturb_timer);
 730         sfq_free(q->ht);
 731         sfq_free(q->slots);
 732         kfree(q->red_parms);
 733 }
 734
 735 static int sfq_init(struct Qdisc *sch, struct nlattr *opt,
 736                     struct netlink_ext_ack *extack)
 737 {
 738         struct sfq_sched_data *q = qdisc_priv(sch);
 739         int i;
 740         int err;
 741
 742         q->sch = sch;
 743         timer_setup(&q->perturb_timer, sfq_perturbation, TIMER_DEFERRABLE);
 744
 745         err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
 746         if (err)
 747                 return err;
 748
 749         for (i = 0; i < SFQ_MAX_DEPTH + 1; i++) {
 750                 q->dep[i].next = i + SFQ_MAX_FLOWS;
 751                 q->dep[i].prev = i + SFQ_MAX_FLOWS;
 752         }
 753
 754         q->limit = SFQ_MAX_DEPTH;
 755         q->maxdepth = SFQ_MAX_DEPTH;
 756         q->cur_depth = 0;
 757         q->tail = NULL;
 758         q->divisor = SFQ_DEFAULT_HASH_DIVISOR;
 759         q->maxflows = SFQ_DEFAULT_FLOWS;
 760         q->quantum = psched_mtu(qdisc_dev(sch));
 761         q->scaled_quantum = SFQ_ALLOT_SIZE(q->quantum);
 762         q->perturb_period = 0;
 763         get_random_bytes(&q->perturbation, sizeof(q->perturbation));
 764
 765         if (opt) {
 766                 int err = sfq_change(sch, opt);
 767                 if (err)
 768                         return err;
 769         }
 770
 771         q->ht = sfq_alloc(sizeof(q->ht[0]) * q->divisor);
 772         q->slots = sfq_alloc(sizeof(q->slots[0]) * q->maxflows);
 773         if (!q->ht || !q->slots) {
 774                 /* Note: sfq_destroy() will be called by our caller */
 775                 return -ENOMEM;
 776         }
 777
 778         for (i = 0; i < q->divisor; i++)
 779                 q->ht[i] = SFQ_EMPTY_SLOT;
 780
 781         for (i = 0; i < q->maxflows; i++) {
 782                 slot_queue_init(&q->slots[i]);
 783                 sfq_link(q, i);
 784         }
 785         if (q->limit >= 1)
 786                 sch->flags |= TCQ_F_CAN_BYPASS;
 787         else
 788                 sch->flags &= ~TCQ_F_CAN_BYPASS;
 789         return 0;
 790 }
 791
 792 static int sfq_dump(struct Qdisc *sch, struct sk_buff *skb)
 793 {
 794         struct sfq_sched_data *q = qdisc_priv(sch);
 795         unsigned char *b = skb_tail_pointer(skb);
 796         struct tc_sfq_qopt_v1 opt;
 797         struct red_parms *p = q->red_parms;
 798
 799         memset(&opt, 0, sizeof(opt));
 800         opt.v0.quantum  = q->quantum;
 801         opt.v0.perturb_period = q->perturb_period / HZ;
 802         opt.v0.limit    = q->limit;
 803         opt.v0.divisor  = q->divisor;
 804         opt.v0.flows    = q->maxflows;
 805         opt.depth       = q->maxdepth;
 806         opt.headdrop    = q->headdrop;
 807
 808         if (p) {
 809                 opt.qth_min     = p->qth_min >> p->Wlog;
 810                 opt.qth_max     = p->qth_max >> p->Wlog;
 811                 opt.Wlog        = p->Wlog;
 812                 opt.Plog        = p->Plog;
 813                 opt.Scell_log   = p->Scell_log;
 814                 opt.max_P       = p->max_P;
 815         }
 816         memcpy(&opt.stats, &q->stats, sizeof(opt.stats));
 817         opt.flags       = q->flags;
 818
 819         if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
 820                 goto nla_put_failure;
 821
 822         return skb->len;
 823
 824 nla_put_failure:
 825         nlmsg_trim(skb, b);
 826         return -1;
 827 }
 828
 829 static struct Qdisc *sfq_leaf(struct Qdisc *sch, unsigned long arg)
 830 {
 831         return NULL;
 832 }
 833
 834 static unsigned long sfq_find(struct Qdisc *sch, u32 classid)
 835 {
 836         return 0;
 837 }
 838
 839 static unsigned long sfq_bind(struct Qdisc *sch, unsigned long parent,
 840                               u32 classid)
 841 {
 842         return 0;
 843 }
 844
 845 static void sfq_unbind(struct Qdisc *q, unsigned long cl)
 846 {
 847 }
 848
 849 static struct tcf_block *sfq_tcf_block(struct Qdisc *sch, unsigned long cl,
 850                                        struct netlink_ext_ack *extack)
 851 {
 852         struct sfq_sched_data *q = qdisc_priv(sch);
 853
 854         if (cl)
 855                 return NULL;
 856         return q->block;
 857 }
 858
 859 static int sfq_dump_class(struct Qdisc *sch, unsigned long cl,
 860                           struct sk_buff *skb, struct tcmsg *tcm)
 861 {
 862         tcm->tcm_handle |= TC_H_MIN(cl);
 863         return 0;
 864 }
 865
 866 static int sfq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
 867                                 struct gnet_dump *d)
 868 {
 869         struct sfq_sched_data *q = qdisc_priv(sch);
 870         sfq_index idx = q->ht[cl - 1];
 871         struct gnet_stats_queue qs = { 0 };
 872         struct tc_sfq_xstats xstats = { 0 };
 873
 874         if (idx != SFQ_EMPTY_SLOT) {
 875                 const struct sfq_slot *slot = &q->slots[idx];
 876
 877                 xstats.allot = slot->allot << SFQ_ALLOT_SHIFT;
 878                 qs.qlen = slot->qlen;
 879                 qs.backlog = slot->backlog;
 880         }
 881         if (gnet_stats_copy_queue(d, NULL, &qs, qs.qlen) < 0)
 882                 return -1;
 883         return gnet_stats_copy_app(d, &xstats, sizeof(xstats));
 884 }
 885
 886 static void sfq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
 887 {
 888         struct sfq_sched_data *q = qdisc_priv(sch);
 889         unsigned int i;
 890
 891         if (arg->stop)
 892                 return;
 893
 894         for (i = 0; i < q->divisor; i++) {
 895                 if (q->ht[i] == SFQ_EMPTY_SLOT ||
 896                     arg->count < arg->skip) {
 897                         arg->count++;
 898                         continue;
 899                 }
 900                 if (arg->fn(sch, i + 1, arg) < 0) {
 901                         arg->stop = 1;
 902                         break;
 903                 }
 904                 arg->count++;
 905         }
 906 }
 907
 908 static const struct Qdisc_class_ops sfq_class_ops = {
 909         .leaf           =       sfq_leaf,
 910         .find           =       sfq_find,
 911         .tcf_block      =       sfq_tcf_block,
 912         .bind_tcf       =       sfq_bind,
 913         .unbind_tcf     =       sfq_unbind,
 914         .dump           =       sfq_dump_class,
 915         .dump_stats     =       sfq_dump_class_stats,
 916         .walk           =       sfq_walk,
 917 };
 918
 919 static struct Qdisc_ops sfq_qdisc_ops __read_mostly = {
 920         .cl_ops         =       &sfq_class_ops,
 921         .id             =       "sfq",
 922         .priv_size      =       sizeof(struct sfq_sched_data),
 923         .enqueue        =       sfq_enqueue,
 924         .dequeue        =       sfq_dequeue,
 925         .peek           =       qdisc_peek_dequeued,
 926         .init           =       sfq_init,
 927         .reset          =       sfq_reset,
 928         .destroy        =       sfq_destroy,
 929         .change         =       NULL,
 930         .dump           =       sfq_dump,
 931         .owner          =       THIS_MODULE,
 932 };
 933
 934 static int __init sfq_module_init(void)
 935 {
 936         return register_qdisc(&sfq_qdisc_ops);
 937 }
 938 static void __exit sfq_module_exit(void)
 939 {
 940         unregister_qdisc(&sfq_qdisc_ops);
 941 }
 942 module_init(sfq_module_init)
 943 module_exit(sfq_module_exit)
 944 MODULE_LICENSE("GPL");