net/sched/sch_cake.c

   1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2
   3 /* COMMON Applications Kept Enhanced (CAKE) discipline
   4  *
   5  * Copyright (C) 2014-2018 Jonathan Morton <chromatix99@gmail.com>
   6  * Copyright (C) 2015-2018 Toke Høiland-Jørgensen <toke@toke.dk>
   7  * Copyright (C) 2014-2018 Dave Täht <dave.taht@gmail.com>
   8  * Copyright (C) 2015-2018 Sebastian Moeller <moeller0@gmx.de>
   9  * (C) 2015-2018 Kevin Darbyshire-Bryant <kevin@darbyshire-bryant.me.uk>
  10  * Copyright (C) 2017-2018 Ryan Mounce <ryan@mounce.com.au>
  11  *
  12  * The CAKE Principles:
  13  *                 (or, how to have your cake and eat it too)
  14  *
  15  * This is a combination of several shaping, AQM and FQ techniques into one
  16  * easy-to-use package:
  17  *
  18  * - An overall bandwidth shaper, to move the bottleneck away from dumb CPE
  19  *   equipment and bloated MACs.  This operates in deficit mode (as in sch_fq),
  20  *   eliminating the need for any sort of burst parameter (eg. token bucket
  21  *   depth).  Burst support is limited to that necessary to overcome scheduling
  22  *   latency.
  23  *
  24  * - A Diffserv-aware priority queue, giving more priority to certain classes,
  25  *   up to a specified fraction of bandwidth.  Above that bandwidth threshold,
  26  *   the priority is reduced to avoid starving other tins.
  27  *
  28  * - Each priority tin has a separate Flow Queue system, to isolate traffic
  29  *   flows from each other.  This prevents a burst on one flow from increasing
  30  *   the delay to another.  Flows are distributed to queues using a
  31  *   set-associative hash function.
  32  *
  33  * - Each queue is actively managed by Cobalt, which is a combination of the
  34  *   Codel and Blue AQM algorithms.  This serves flows fairly, and signals
  35  *   congestion early via ECN (if available) and/or packet drops, to keep
  36  *   latency low.  The codel parameters are auto-tuned based on the bandwidth
  37  *   setting, as is necessary at low bandwidths.
  38  *
  39  * The configuration parameters are kept deliberately simple for ease of use.
  40  * Everything has sane defaults.  Complete generality of configuration is *not*
  41  * a goal.
  42  *
  43  * The priority queue operates according to a weighted DRR scheme, combined with
  44  * a bandwidth tracker which reuses the shaper logic to detect which side of the
  45  * bandwidth sharing threshold the tin is operating.  This determines whether a
  46  * priority-based weight (high) or a bandwidth-based weight (low) is used for
  47  * that tin in the current pass.
  48  *
  49  * This qdisc was inspired by Eric Dumazet's fq_codel code, which he kindly
  50  * granted us permission to leverage.
  51  */
  52
  53 #include <linux/module.h>
  54 #include <linux/types.h>
  55 #include <linux/kernel.h>
  56 #include <linux/jiffies.h>
  57 #include <linux/string.h>
  58 #include <linux/in.h>
  59 #include <linux/errno.h>
  60 #include <linux/init.h>
  61 #include <linux/skbuff.h>
  62 #include <linux/jhash.h>
  63 #include <linux/slab.h>
  64 #include <linux/vmalloc.h>
  65 #include <linux/reciprocal_div.h>
  66 #include <net/netlink.h>
  67 #include <linux/if_vlan.h>
  68 #include <net/pkt_sched.h>
  69 #include <net/pkt_cls.h>
  70 #include <net/tcp.h>
  71 #include <net/flow_dissector.h>
  72
  73 #if IS_ENABLED(CONFIG_NF_CONNTRACK)
  74 #include <net/netfilter/nf_conntrack_core.h>
  75 #endif
  76
  77 #define CAKE_SET_WAYS (8)
  78 #define CAKE_MAX_TINS (8)
  79 #define CAKE_QUEUES (1024)
  80 #define CAKE_FLOW_MASK 63
  81 #define CAKE_FLOW_NAT_FLAG 64
  82
  83 /* struct cobalt_params - contains codel and blue parameters
  84  * @interval:   codel initial drop rate
  85  * @target:     maximum persistent sojourn time & blue update rate
  86  * @mtu_time:   serialisation delay of maximum-size packet
  87  * @p_inc:      increment of blue drop probability (0.32 fxp)
  88  * @p_dec:      decrement of blue drop probability (0.32 fxp)
  89  */
  90 struct cobalt_params {
  91         u64     interval;
  92         u64     target;
  93         u64     mtu_time;
  94         u32     p_inc;
  95         u32     p_dec;
  96 };
  97
  98 /* struct cobalt_vars - contains codel and blue variables
  99  * @count:              codel dropping frequency
 100  * @rec_inv_sqrt:       reciprocal value of sqrt(count) >> 1
 101  * @drop_next:          time to drop next packet, or when we dropped last
 102  * @blue_timer:         Blue time to next drop
 103  * @p_drop:             BLUE drop probability (0.32 fxp)
 104  * @dropping:           set if in dropping state
 105  * @ecn_marked:         set if marked
 106  */
 107 struct cobalt_vars {
 108         u32     count;
 109         u32     rec_inv_sqrt;
 110         ktime_t drop_next;
 111         ktime_t blue_timer;
 112         u32     p_drop;
 113         bool    dropping;
 114         bool    ecn_marked;
 115 };
 116
 117 enum {
 118         CAKE_SET_NONE = 0,
 119         CAKE_SET_SPARSE,
 120         CAKE_SET_SPARSE_WAIT, /* counted in SPARSE, actually in BULK */
 121         CAKE_SET_BULK,
 122         CAKE_SET_DECAYING
 123 };
 124
 125 struct cake_flow {
 126         /* this stuff is all needed per-flow at dequeue time */
 127         struct sk_buff    *head;
 128         struct sk_buff    *tail;
 129         struct list_head  flowchain;
 130         s32               deficit;
 131         u32               dropped;
 132         struct cobalt_vars cvars;
 133         u16               srchost; /* index into cake_host table */
 134         u16               dsthost;
 135         u8                set;
 136 }; /* please try to keep this structure <= 64 bytes */
 137
 138 struct cake_host {
 139         u32 srchost_tag;
 140         u32 dsthost_tag;
 141         u16 srchost_bulk_flow_count;
 142         u16 dsthost_bulk_flow_count;
 143 };
 144
 145 struct cake_heap_entry {
 146         u16 t:3, b:10;
 147 };
 148
 149 struct cake_tin_data {
 150         struct cake_flow flows[CAKE_QUEUES];
 151         u32     backlogs[CAKE_QUEUES];
 152         u32     tags[CAKE_QUEUES]; /* for set association */
 153         u16     overflow_idx[CAKE_QUEUES];
 154         struct cake_host hosts[CAKE_QUEUES]; /* for triple isolation */
 155         u16     flow_quantum;
 156
 157         struct cobalt_params cparams;
 158         u32     drop_overlimit;
 159         u16     bulk_flow_count;
 160         u16     sparse_flow_count;
 161         u16     decaying_flow_count;
 162         u16     unresponsive_flow_count;
 163
 164         u32     max_skblen;
 165
 166         struct list_head new_flows;
 167         struct list_head old_flows;
 168         struct list_head decaying_flows;
 169
 170         /* time_next = time_this + ((len * rate_ns) >> rate_shft) */
 171         ktime_t time_next_packet;
 172         u64     tin_rate_ns;
 173         u64     tin_rate_bps;
 174         u16     tin_rate_shft;
 175
 176         u16     tin_quantum_prio;
 177         u16     tin_quantum_band;
 178         s32     tin_deficit;
 179         u32     tin_backlog;
 180         u32     tin_dropped;
 181         u32     tin_ecn_mark;
 182
 183         u32     packets;
 184         u64     bytes;
 185
 186         u32     ack_drops;
 187
 188         /* moving averages */
 189         u64 avge_delay;
 190         u64 peak_delay;
 191         u64 base_delay;
 192
 193         /* hash function stats */
 194         u32     way_directs;
 195         u32     way_hits;
 196         u32     way_misses;
 197         u32     way_collisions;
 198 }; /* number of tins is small, so size of this struct doesn't matter much */
 199
 200 struct cake_sched_data {
 201         struct tcf_proto __rcu *filter_list; /* optional external classifier */
 202         struct tcf_block *block;
 203         struct cake_tin_data *tins;
 204
 205         struct cake_heap_entry overflow_heap[CAKE_QUEUES * CAKE_MAX_TINS];
 206         u16             overflow_timeout;
 207
 208         u16             tin_cnt;
 209         u8              tin_mode;
 210         u8              flow_mode;
 211         u8              ack_filter;
 212         u8              atm_mode;
 213
 214         u32             fwmark_mask;
 215         u16             fwmark_shft;
 216
 217         /* time_next = time_this + ((len * rate_ns) >> rate_shft) */
 218         u16             rate_shft;
 219         ktime_t         time_next_packet;
 220         ktime_t         failsafe_next_packet;
 221         u64             rate_ns;
 222         u64             rate_bps;
 223         u16             rate_flags;
 224         s16             rate_overhead;
 225         u16             rate_mpu;
 226         u64             interval;
 227         u64             target;
 228
 229         /* resource tracking */
 230         u32             buffer_used;
 231         u32             buffer_max_used;
 232         u32             buffer_limit;
 233         u32             buffer_config_limit;
 234
 235         /* indices for dequeue */
 236         u16             cur_tin;
 237         u16             cur_flow;
 238
 239         struct qdisc_watchdog watchdog;
 240         const u8        *tin_index;
 241         const u8        *tin_order;
 242
 243         /* bandwidth capacity estimate */
 244         ktime_t         last_packet_time;
 245         ktime_t         avg_window_begin;
 246         u64             avg_packet_interval;
 247         u64             avg_window_bytes;
 248         u64             avg_peak_bandwidth;
 249         ktime_t         last_reconfig_time;
 250
 251         /* packet length stats */
 252         u32             avg_netoff;
 253         u16             max_netlen;
 254         u16             max_adjlen;
 255         u16             min_netlen;
 256         u16             min_adjlen;
 257 };
 258
 259 enum {
 260         CAKE_FLAG_OVERHEAD         = BIT(0),
 261         CAKE_FLAG_AUTORATE_INGRESS = BIT(1),
 262         CAKE_FLAG_INGRESS          = BIT(2),
 263         CAKE_FLAG_WASH             = BIT(3),
 264         CAKE_FLAG_SPLIT_GSO        = BIT(4)
 265 };
 266
 267 /* COBALT operates the Codel and BLUE algorithms in parallel, in order to
 268  * obtain the best features of each.  Codel is excellent on flows which
 269  * respond to congestion signals in a TCP-like way.  BLUE is more effective on
 270  * unresponsive flows.
 271  */
 272
 273 struct cobalt_skb_cb {
 274         ktime_t enqueue_time;
 275         u32     adjusted_len;
 276 };
 277
 278 static u64 us_to_ns(u64 us)
 279 {
 280         return us * NSEC_PER_USEC;
 281 }
 282
 283 static struct cobalt_skb_cb *get_cobalt_cb(const struct sk_buff *skb)
 284 {
 285         qdisc_cb_private_validate(skb, sizeof(struct cobalt_skb_cb));
 286         return (struct cobalt_skb_cb *)qdisc_skb_cb(skb)->data;
 287 }
 288
 289 static ktime_t cobalt_get_enqueue_time(const struct sk_buff *skb)
 290 {
 291         return get_cobalt_cb(skb)->enqueue_time;
 292 }
 293
 294 static void cobalt_set_enqueue_time(struct sk_buff *skb,
 295                                     ktime_t now)
 296 {
 297         get_cobalt_cb(skb)->enqueue_time = now;
 298 }
 299
 300 static u16 quantum_div[CAKE_QUEUES + 1] = {0};
 301
 302 /* Diffserv lookup tables */
 303
 304 static const u8 precedence[] = {
 305         0, 0, 0, 0, 0, 0, 0, 0,
 306         1, 1, 1, 1, 1, 1, 1, 1,
 307         2, 2, 2, 2, 2, 2, 2, 2,
 308         3, 3, 3, 3, 3, 3, 3, 3,
 309         4, 4, 4, 4, 4, 4, 4, 4,
 310         5, 5, 5, 5, 5, 5, 5, 5,
 311         6, 6, 6, 6, 6, 6, 6, 6,
 312         7, 7, 7, 7, 7, 7, 7, 7,
 313 };
 314
 315 static const u8 diffserv8[] = {
 316         2, 5, 1, 2, 4, 2, 2, 2,
 317         0, 2, 1, 2, 1, 2, 1, 2,
 318         5, 2, 4, 2, 4, 2, 4, 2,
 319         3, 2, 3, 2, 3, 2, 3, 2,
 320         6, 2, 3, 2, 3, 2, 3, 2,
 321         6, 2, 2, 2, 6, 2, 6, 2,
 322         7, 2, 2, 2, 2, 2, 2, 2,
 323         7, 2, 2, 2, 2, 2, 2, 2,
 324 };
 325
 326 static const u8 diffserv4[] = {
 327         0, 2, 0, 0, 2, 0, 0, 0,
 328         1, 0, 0, 0, 0, 0, 0, 0,
 329         2, 0, 2, 0, 2, 0, 2, 0,
 330         2, 0, 2, 0, 2, 0, 2, 0,
 331         3, 0, 2, 0, 2, 0, 2, 0,
 332         3, 0, 0, 0, 3, 0, 3, 0,
 333         3, 0, 0, 0, 0, 0, 0, 0,
 334         3, 0, 0, 0, 0, 0, 0, 0,
 335 };
 336
 337 static const u8 diffserv3[] = {
 338         0, 0, 0, 0, 2, 0, 0, 0,
 339         1, 0, 0, 0, 0, 0, 0, 0,
 340         0, 0, 0, 0, 0, 0, 0, 0,
 341         0, 0, 0, 0, 0, 0, 0, 0,
 342         0, 0, 0, 0, 0, 0, 0, 0,
 343         0, 0, 0, 0, 2, 0, 2, 0,
 344         2, 0, 0, 0, 0, 0, 0, 0,
 345         2, 0, 0, 0, 0, 0, 0, 0,
 346 };
 347
 348 static const u8 besteffort[] = {
 349         0, 0, 0, 0, 0, 0, 0, 0,
 350         0, 0, 0, 0, 0, 0, 0, 0,
 351         0, 0, 0, 0, 0, 0, 0, 0,
 352         0, 0, 0, 0, 0, 0, 0, 0,
 353         0, 0, 0, 0, 0, 0, 0, 0,
 354         0, 0, 0, 0, 0, 0, 0, 0,
 355         0, 0, 0, 0, 0, 0, 0, 0,
 356         0, 0, 0, 0, 0, 0, 0, 0,
 357 };
 358
 359 /* tin priority order for stats dumping */
 360
 361 static const u8 normal_order[] = {0, 1, 2, 3, 4, 5, 6, 7};
 362 static const u8 bulk_order[] = {1, 0, 2, 3};
 363
 364 #define REC_INV_SQRT_CACHE (16)
 365 static u32 cobalt_rec_inv_sqrt_cache[REC_INV_SQRT_CACHE] = {0};
 366
 367 /* http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
 368  * new_invsqrt = (invsqrt / 2) * (3 - count * invsqrt^2)
 369  *
 370  * Here, invsqrt is a fixed point number (< 1.0), 32bit mantissa, aka Q0.32
 371  */
 372
 373 static void cobalt_newton_step(struct cobalt_vars *vars)
 374 {
 375         u32 invsqrt, invsqrt2;
 376         u64 val;
 377
 378         invsqrt = vars->rec_inv_sqrt;
 379         invsqrt2 = ((u64)invsqrt * invsqrt) >> 32;
 380         val = (3LL << 32) - ((u64)vars->count * invsqrt2);
 381
 382         val >>= 2; /* avoid overflow in following multiply */
 383         val = (val * invsqrt) >> (32 - 2 + 1);
 384
 385         vars->rec_inv_sqrt = val;
 386 }
 387
 388 static void cobalt_invsqrt(struct cobalt_vars *vars)
 389 {
 390         if (vars->count < REC_INV_SQRT_CACHE)
 391                 vars->rec_inv_sqrt = cobalt_rec_inv_sqrt_cache[vars->count];
 392         else
 393                 cobalt_newton_step(vars);
 394 }
 395
 396 /* There is a big difference in timing between the accurate values placed in
 397  * the cache and the approximations given by a single Newton step for small
 398  * count values, particularly when stepping from count 1 to 2 or vice versa.
 399  * Above 16, a single Newton step gives sufficient accuracy in either
 400  * direction, given the precision stored.
 401  *
 402  * The magnitude of the error when stepping up to count 2 is such as to give
 403  * the value that *should* have been produced at count 4.
 404  */
 405
 406 static void cobalt_cache_init(void)
 407 {
 408         struct cobalt_vars v;
 409
 410         memset(&v, 0, sizeof(v));
 411         v.rec_inv_sqrt = ~0U;
 412         cobalt_rec_inv_sqrt_cache[0] = v.rec_inv_sqrt;
 413
 414         for (v.count = 1; v.count < REC_INV_SQRT_CACHE; v.count++) {
 415                 cobalt_newton_step(&v);
 416                 cobalt_newton_step(&v);
 417                 cobalt_newton_step(&v);
 418                 cobalt_newton_step(&v);
 419
 420                 cobalt_rec_inv_sqrt_cache[v.count] = v.rec_inv_sqrt;
 421         }
 422 }
 423
 424 static void cobalt_vars_init(struct cobalt_vars *vars)
 425 {
 426         memset(vars, 0, sizeof(*vars));
 427
 428         if (!cobalt_rec_inv_sqrt_cache[0]) {
 429                 cobalt_cache_init();
 430                 cobalt_rec_inv_sqrt_cache[0] = ~0;
 431         }
 432 }
 433
 434 /* CoDel control_law is t + interval/sqrt(count)
 435  * We maintain in rec_inv_sqrt the reciprocal value of sqrt(count) to avoid
 436  * both sqrt() and divide operation.
 437  */
 438 static ktime_t cobalt_control(ktime_t t,
 439                               u64 interval,
 440                               u32 rec_inv_sqrt)
 441 {
 442         return ktime_add_ns(t, reciprocal_scale(interval,
 443                                                 rec_inv_sqrt));
 444 }
 445
 446 /* Call this when a packet had to be dropped due to queue overflow.  Returns
 447  * true if the BLUE state was quiescent before but active after this call.
 448  */
 449 static bool cobalt_queue_full(struct cobalt_vars *vars,
 450                               struct cobalt_params *p,
 451                               ktime_t now)
 452 {
 453         bool up = false;
 454
 455         if (ktime_to_ns(ktime_sub(now, vars->blue_timer)) > p->target) {
 456                 up = !vars->p_drop;
 457                 vars->p_drop += p->p_inc;
 458                 if (vars->p_drop < p->p_inc)
 459                         vars->p_drop = ~0;
 460                 vars->blue_timer = now;
 461         }
 462         vars->dropping = true;
 463         vars->drop_next = now;
 464         if (!vars->count)
 465                 vars->count = 1;
 466
 467         return up;
 468 }
 469
 470 /* Call this when the queue was serviced but turned out to be empty.  Returns
 471  * true if the BLUE state was active before but quiescent after this call.
 472  */
 473 static bool cobalt_queue_empty(struct cobalt_vars *vars,
 474                                struct cobalt_params *p,
 475                                ktime_t now)
 476 {
 477         bool down = false;
 478
 479         if (vars->p_drop &&
 480             ktime_to_ns(ktime_sub(now, vars->blue_timer)) > p->target) {
 481                 if (vars->p_drop < p->p_dec)
 482                         vars->p_drop = 0;
 483                 else
 484                         vars->p_drop -= p->p_dec;
 485                 vars->blue_timer = now;
 486                 down = !vars->p_drop;
 487         }
 488         vars->dropping = false;
 489
 490         if (vars->count && ktime_to_ns(ktime_sub(now, vars->drop_next)) >= 0) {
 491                 vars->count--;
 492                 cobalt_invsqrt(vars);
 493                 vars->drop_next = cobalt_control(vars->drop_next,
 494                                                  p->interval,
 495                                                  vars->rec_inv_sqrt);
 496         }
 497
 498         return down;
 499 }
 500
 501 /* Call this with a freshly dequeued packet for possible congestion marking.
 502  * Returns true as an instruction to drop the packet, false for delivery.
 503  */
 504 static bool cobalt_should_drop(struct cobalt_vars *vars,
 505                                struct cobalt_params *p,
 506                                ktime_t now,
 507                                struct sk_buff *skb,
 508                                u32 bulk_flows)
 509 {
 510         bool next_due, over_target, drop = false;
 511         ktime_t schedule;
 512         u64 sojourn;
 513
 514 /* The 'schedule' variable records, in its sign, whether 'now' is before or
 515  * after 'drop_next'.  This allows 'drop_next' to be updated before the next
 516  * scheduling decision is actually branched, without destroying that
 517  * information.  Similarly, the first 'schedule' value calculated is preserved
 518  * in the boolean 'next_due'.
 519  *
 520  * As for 'drop_next', we take advantage of the fact that 'interval' is both
 521  * the delay between first exceeding 'target' and the first signalling event,
 522  * *and* the scaling factor for the signalling frequency.  It's therefore very
 523  * natural to use a single mechanism for both purposes, and eliminates a
 524  * significant amount of reference Codel's spaghetti code.  To help with this,
 525  * both the '0' and '1' entries in the invsqrt cache are 0xFFFFFFFF, as close
 526  * as possible to 1.0 in fixed-point.
 527  */
 528
 529         sojourn = ktime_to_ns(ktime_sub(now, cobalt_get_enqueue_time(skb)));
 530         schedule = ktime_sub(now, vars->drop_next);
 531         over_target = sojourn > p->target &&
 532                       sojourn > p->mtu_time * bulk_flows * 2 &&
 533                       sojourn > p->mtu_time * 4;
 534         next_due = vars->count && ktime_to_ns(schedule) >= 0;
 535
 536         vars->ecn_marked = false;
 537
 538         if (over_target) {
 539                 if (!vars->dropping) {
 540                         vars->dropping = true;
 541                         vars->drop_next = cobalt_control(now,
 542                                                          p->interval,
 543                                                          vars->rec_inv_sqrt);
 544                 }
 545                 if (!vars->count)
 546                         vars->count = 1;
 547         } else if (vars->dropping) {
 548                 vars->dropping = false;
 549         }
 550
 551         if (next_due && vars->dropping) {
 552                 /* Use ECN mark if possible, otherwise drop */
 553                 drop = !(vars->ecn_marked = INET_ECN_set_ce(skb));
 554
 555                 vars->count++;
 556                 if (!vars->count)
 557                         vars->count--;
 558                 cobalt_invsqrt(vars);
 559                 vars->drop_next = cobalt_control(vars->drop_next,
 560                                                  p->interval,
 561                                                  vars->rec_inv_sqrt);
 562                 schedule = ktime_sub(now, vars->drop_next);
 563         } else {
 564                 while (next_due) {
 565                         vars->count--;
 566                         cobalt_invsqrt(vars);
 567                         vars->drop_next = cobalt_control(vars->drop_next,
 568                                                          p->interval,
 569                                                          vars->rec_inv_sqrt);
 570                         schedule = ktime_sub(now, vars->drop_next);
 571                         next_due = vars->count && ktime_to_ns(schedule) >= 0;
 572                 }
 573         }
 574
 575         /* Simple BLUE implementation.  Lack of ECN is deliberate. */
 576         if (vars->p_drop)
 577                 drop |= (prandom_u32() < vars->p_drop);
 578
 579         /* Overload the drop_next field as an activity timeout */
 580         if (!vars->count)
 581                 vars->drop_next = ktime_add_ns(now, p->interval);
 582         else if (ktime_to_ns(schedule) > 0 && !drop)
 583                 vars->drop_next = now;
 584
 585         return drop;
 586 }
 587
 588 static void cake_update_flowkeys(struct flow_keys *keys,
 589                                  const struct sk_buff *skb)
 590 {
 591 #if IS_ENABLED(CONFIG_NF_CONNTRACK)
 592         struct nf_conntrack_tuple tuple = {};
 593         bool rev = !skb->_nfct;
 594
 595         if (skb_protocol(skb, true) != htons(ETH_P_IP))
 596                 return;
 597
 598         if (!nf_ct_get_tuple_skb(&tuple, skb))
 599                 return;
 600
 601         keys->addrs.v4addrs.src = rev ? tuple.dst.u3.ip : tuple.src.u3.ip;
 602         keys->addrs.v4addrs.dst = rev ? tuple.src.u3.ip : tuple.dst.u3.ip;
 603
 604         if (keys->ports.ports) {
 605                 keys->ports.src = rev ? tuple.dst.u.all : tuple.src.u.all;
 606                 keys->ports.dst = rev ? tuple.src.u.all : tuple.dst.u.all;
 607         }
 608 #endif
 609 }
 610
 611 /* Cake has several subtle multiple bit settings. In these cases you
 612  *  would be matching triple isolate mode as well.
 613  */
 614
 615 static bool cake_dsrc(int flow_mode)
 616 {
 617         return (flow_mode & CAKE_FLOW_DUAL_SRC) == CAKE_FLOW_DUAL_SRC;
 618 }
 619
 620 static bool cake_ddst(int flow_mode)
 621 {
 622         return (flow_mode & CAKE_FLOW_DUAL_DST) == CAKE_FLOW_DUAL_DST;
 623 }
 624
 625 static u32 cake_hash(struct cake_tin_data *q, const struct sk_buff *skb,
 626                      int flow_mode, u16 flow_override, u16 host_override)
 627 {
 628         u32 flow_hash = 0, srchost_hash = 0, dsthost_hash = 0;
 629         u16 reduced_hash, srchost_idx, dsthost_idx;
 630         struct flow_keys keys, host_keys;
 631
 632         if (unlikely(flow_mode == CAKE_FLOW_NONE))
 633                 return 0;
 634
 635         /* If both overrides are set we can skip packet dissection entirely */
 636         if ((flow_override || !(flow_mode & CAKE_FLOW_FLOWS)) &&
 637             (host_override || !(flow_mode & CAKE_FLOW_HOSTS)))
 638                 goto skip_hash;
 639
 640         skb_flow_dissect_flow_keys(skb, &keys,
 641                                    FLOW_DISSECTOR_F_STOP_AT_FLOW_LABEL);
 642
 643         if (flow_mode & CAKE_FLOW_NAT_FLAG)
 644                 cake_update_flowkeys(&keys, skb);
 645
 646         /* flow_hash_from_keys() sorts the addresses by value, so we have
 647          * to preserve their order in a separate data structure to treat
 648          * src and dst host addresses as independently selectable.
 649          */
 650         host_keys = keys;
 651         host_keys.ports.ports     = 0;
 652         host_keys.basic.ip_proto  = 0;
 653         host_keys.keyid.keyid     = 0;
 654         host_keys.tags.flow_label = 0;
 655
 656         switch (host_keys.control.addr_type) {
 657         case FLOW_DISSECTOR_KEY_IPV4_ADDRS:
 658                 host_keys.addrs.v4addrs.src = 0;
 659                 dsthost_hash = flow_hash_from_keys(&host_keys);
 660                 host_keys.addrs.v4addrs.src = keys.addrs.v4addrs.src;
 661                 host_keys.addrs.v4addrs.dst = 0;
 662                 srchost_hash = flow_hash_from_keys(&host_keys);
 663                 break;
 664
 665         case FLOW_DISSECTOR_KEY_IPV6_ADDRS:
 666                 memset(&host_keys.addrs.v6addrs.src, 0,
 667                        sizeof(host_keys.addrs.v6addrs.src));
 668                 dsthost_hash = flow_hash_from_keys(&host_keys);
 669                 host_keys.addrs.v6addrs.src = keys.addrs.v6addrs.src;
 670                 memset(&host_keys.addrs.v6addrs.dst, 0,
 671                        sizeof(host_keys.addrs.v6addrs.dst));
 672                 srchost_hash = flow_hash_from_keys(&host_keys);
 673                 break;
 674
 675         default:
 676                 dsthost_hash = 0;
 677                 srchost_hash = 0;
 678         }
 679
 680         /* This *must* be after the above switch, since as a
 681          * side-effect it sorts the src and dst addresses.
 682          */
 683         if (flow_mode & CAKE_FLOW_FLOWS)
 684                 flow_hash = flow_hash_from_keys(&keys);
 685
 686 skip_hash:
 687         if (flow_override)
 688                 flow_hash = flow_override - 1;
 689         if (host_override) {
 690                 dsthost_hash = host_override - 1;
 691                 srchost_hash = host_override - 1;
 692         }
 693
 694         if (!(flow_mode & CAKE_FLOW_FLOWS)) {
 695                 if (flow_mode & CAKE_FLOW_SRC_IP)
 696                         flow_hash ^= srchost_hash;
 697
 698                 if (flow_mode & CAKE_FLOW_DST_IP)
 699                         flow_hash ^= dsthost_hash;
 700         }
 701
 702         reduced_hash = flow_hash % CAKE_QUEUES;
 703
 704         /* set-associative hashing */
 705         /* fast path if no hash collision (direct lookup succeeds) */
 706         if (likely(q->tags[reduced_hash] == flow_hash &&
 707                    q->flows[reduced_hash].set)) {
 708                 q->way_directs++;
 709         } else {
 710                 u32 inner_hash = reduced_hash % CAKE_SET_WAYS;
 711                 u32 outer_hash = reduced_hash - inner_hash;
 712                 bool allocate_src = false;
 713                 bool allocate_dst = false;
 714                 u32 i, k;
 715
 716                 /* check if any active queue in the set is reserved for
 717                  * this flow.
 718                  */
 719                 for (i = 0, k = inner_hash; i < CAKE_SET_WAYS;
 720                      i++, k = (k + 1) % CAKE_SET_WAYS) {
 721                         if (q->tags[outer_hash + k] == flow_hash) {
 722                                 if (i)
 723                                         q->way_hits++;
 724
 725                                 if (!q->flows[outer_hash + k].set) {
 726                                         /* need to increment host refcnts */
 727                                         allocate_src = cake_dsrc(flow_mode);
 728                                         allocate_dst = cake_ddst(flow_mode);
 729                                 }
 730
 731                                 goto found;
 732                         }
 733                 }
 734
 735                 /* no queue is reserved for this flow, look for an
 736                  * empty one.
 737                  */
 738                 for (i = 0; i < CAKE_SET_WAYS;
 739                          i++, k = (k + 1) % CAKE_SET_WAYS) {
 740                         if (!q->flows[outer_hash + k].set) {
 741                                 q->way_misses++;
 742                                 allocate_src = cake_dsrc(flow_mode);
 743                                 allocate_dst = cake_ddst(flow_mode);
 744                                 goto found;
 745                         }
 746                 }
 747
 748                 /* With no empty queues, default to the original
 749                  * queue, accept the collision, update the host tags.
 750                  */
 751                 q->way_collisions++;
 752                 if (q->flows[outer_hash + k].set == CAKE_SET_BULK) {
 753                         q->hosts[q->flows[reduced_hash].srchost].srchost_bulk_flow_count--;
 754                         q->hosts[q->flows[reduced_hash].dsthost].dsthost_bulk_flow_count--;
 755                 }
 756                 allocate_src = cake_dsrc(flow_mode);
 757                 allocate_dst = cake_ddst(flow_mode);
 758 found:
 759                 /* reserve queue for future packets in same flow */
 760                 reduced_hash = outer_hash + k;
 761                 q->tags[reduced_hash] = flow_hash;
 762
 763                 if (allocate_src) {
 764                         srchost_idx = srchost_hash % CAKE_QUEUES;
 765                         inner_hash = srchost_idx % CAKE_SET_WAYS;
 766                         outer_hash = srchost_idx - inner_hash;
 767                         for (i = 0, k = inner_hash; i < CAKE_SET_WAYS;
 768                                 i++, k = (k + 1) % CAKE_SET_WAYS) {
 769                                 if (q->hosts[outer_hash + k].srchost_tag ==
 770                                     srchost_hash)
 771                                         goto found_src;
 772                         }
 773                         for (i = 0; i < CAKE_SET_WAYS;
 774                                 i++, k = (k + 1) % CAKE_SET_WAYS) {
 775                                 if (!q->hosts[outer_hash + k].srchost_bulk_flow_count)
 776                                         break;
 777                         }
 778                         q->hosts[outer_hash + k].srchost_tag = srchost_hash;
 779 found_src:
 780                         srchost_idx = outer_hash + k;
 781                         if (q->flows[reduced_hash].set == CAKE_SET_BULK)
 782                                 q->hosts[srchost_idx].srchost_bulk_flow_count++;
 783                         q->flows[reduced_hash].srchost = srchost_idx;
 784                 }
 785
 786                 if (allocate_dst) {
 787                         dsthost_idx = dsthost_hash % CAKE_QUEUES;
 788                         inner_hash = dsthost_idx % CAKE_SET_WAYS;
 789                         outer_hash = dsthost_idx - inner_hash;
 790                         for (i = 0, k = inner_hash; i < CAKE_SET_WAYS;
 791                              i++, k = (k + 1) % CAKE_SET_WAYS) {
 792                                 if (q->hosts[outer_hash + k].dsthost_tag ==
 793                                     dsthost_hash)
 794                                         goto found_dst;
 795                         }
 796                         for (i = 0; i < CAKE_SET_WAYS;
 797                              i++, k = (k + 1) % CAKE_SET_WAYS) {
 798                                 if (!q->hosts[outer_hash + k].dsthost_bulk_flow_count)
 799                                         break;
 800                         }
 801                         q->hosts[outer_hash + k].dsthost_tag = dsthost_hash;
 802 found_dst:
 803                         dsthost_idx = outer_hash + k;
 804                         if (q->flows[reduced_hash].set == CAKE_SET_BULK)
 805                                 q->hosts[dsthost_idx].dsthost_bulk_flow_count++;
 806                         q->flows[reduced_hash].dsthost = dsthost_idx;
 807                 }
 808         }
 809
 810         return reduced_hash;
 811 }
 812
 813 /* helper functions : might be changed when/if skb use a standard list_head */
 814 /* remove one skb from head of slot queue */
 815
 816 static struct sk_buff *dequeue_head(struct cake_flow *flow)
 817 {
 818         struct sk_buff *skb = flow->head;
 819
 820         if (skb) {
 821                 flow->head = skb->next;
 822                 skb_mark_not_on_list(skb);
 823         }
 824
 825         return skb;
 826 }
 827
 828 /* add skb to flow queue (tail add) */
 829
 830 static void flow_queue_add(struct cake_flow *flow, struct sk_buff *skb)
 831 {
 832         if (!flow->head)
 833                 flow->head = skb;
 834         else
 835                 flow->tail->next = skb;
 836         flow->tail = skb;
 837         skb->next = NULL;
 838 }
 839
 840 static struct iphdr *cake_get_iphdr(const struct sk_buff *skb,
 841                                     struct ipv6hdr *buf)
 842 {
 843         unsigned int offset = skb_network_offset(skb);
 844         struct iphdr *iph;
 845
 846         iph = skb_header_pointer(skb, offset, sizeof(struct iphdr), buf);
 847
 848         if (!iph)
 849                 return NULL;
 850
 851         if (iph->version == 4 && iph->protocol == IPPROTO_IPV6)
 852                 return skb_header_pointer(skb, offset + iph->ihl * 4,
 853                                           sizeof(struct ipv6hdr), buf);
 854
 855         else if (iph->version == 4)
 856                 return iph;
 857
 858         else if (iph->version == 6)
 859                 return skb_header_pointer(skb, offset, sizeof(struct ipv6hdr),
 860                                           buf);
 861
 862         return NULL;
 863 }
 864
 865 static struct tcphdr *cake_get_tcphdr(const struct sk_buff *skb,
 866                                       void *buf, unsigned int bufsize)
 867 {
 868         unsigned int offset = skb_network_offset(skb);
 869         const struct ipv6hdr *ipv6h;
 870         const struct tcphdr *tcph;
 871         const struct iphdr *iph;
 872         struct ipv6hdr _ipv6h;
 873         struct tcphdr _tcph;
 874
 875         ipv6h = skb_header_pointer(skb, offset, sizeof(_ipv6h), &_ipv6h);
 876
 877         if (!ipv6h)
 878                 return NULL;
 879
 880         if (ipv6h->version == 4) {
 881                 iph = (struct iphdr *)ipv6h;
 882                 offset += iph->ihl * 4;
 883
 884                 /* special-case 6in4 tunnelling, as that is a common way to get
 885                  * v6 connectivity in the home
 886                  */
 887                 if (iph->protocol == IPPROTO_IPV6) {
 888                         ipv6h = skb_header_pointer(skb, offset,
 889                                                    sizeof(_ipv6h), &_ipv6h);
 890
 891                         if (!ipv6h || ipv6h->nexthdr != IPPROTO_TCP)
 892                                 return NULL;
 893
 894                         offset += sizeof(struct ipv6hdr);
 895
 896                 } else if (iph->protocol != IPPROTO_TCP) {
 897                         return NULL;
 898                 }
 899
 900         } else if (ipv6h->version == 6) {
 901                 if (ipv6h->nexthdr != IPPROTO_TCP)
 902                         return NULL;
 903
 904                 offset += sizeof(struct ipv6hdr);
 905         } else {
 906                 return NULL;
 907         }
 908
 909         tcph = skb_header_pointer(skb, offset, sizeof(_tcph), &_tcph);
 910         if (!tcph || tcph->doff < 5)
 911                 return NULL;
 912
 913         return skb_header_pointer(skb, offset,
 914                                   min(__tcp_hdrlen(tcph), bufsize), buf);
 915 }
 916
 917 static const void *cake_get_tcpopt(const struct tcphdr *tcph,
 918                                    int code, int *oplen)
 919 {
 920         /* inspired by tcp_parse_options in tcp_input.c */
 921         int length = __tcp_hdrlen(tcph) - sizeof(struct tcphdr);
 922         const u8 *ptr = (const u8 *)(tcph + 1);
 923
 924         while (length > 0) {
 925                 int opcode = *ptr++;
 926                 int opsize;
 927
 928                 if (opcode == TCPOPT_EOL)
 929                         break;
 930                 if (opcode == TCPOPT_NOP) {
 931                         length--;
 932                         continue;
 933                 }
 934                 if (length < 2)
 935                         break;
 936                 opsize = *ptr++;
 937                 if (opsize < 2 || opsize > length)
 938                         break;
 939
 940                 if (opcode == code) {
 941                         *oplen = opsize;
 942                         return ptr;
 943                 }
 944
 945                 ptr += opsize - 2;
 946                 length -= opsize;
 947         }
 948
 949         return NULL;
 950 }
 951
 952 /* Compare two SACK sequences. A sequence is considered greater if it SACKs more
 953  * bytes than the other. In the case where both sequences ACKs bytes that the
 954  * other doesn't, A is considered greater. DSACKs in A also makes A be
 955  * considered greater.
 956  *
 957  * @return -1, 0 or 1 as normal compare functions
 958  */
 959 static int cake_tcph_sack_compare(const struct tcphdr *tcph_a,
 960                                   const struct tcphdr *tcph_b)
 961 {
 962         const struct tcp_sack_block_wire *sack_a, *sack_b;
 963         u32 ack_seq_a = ntohl(tcph_a->ack_seq);
 964         u32 bytes_a = 0, bytes_b = 0;
 965         int oplen_a, oplen_b;
 966         bool first = true;
 967
 968         sack_a = cake_get_tcpopt(tcph_a, TCPOPT_SACK, &oplen_a);
 969         sack_b = cake_get_tcpopt(tcph_b, TCPOPT_SACK, &oplen_b);
 970
 971         /* pointers point to option contents */
 972         oplen_a -= TCPOLEN_SACK_BASE;
 973         oplen_b -= TCPOLEN_SACK_BASE;
 974
 975         if (sack_a && oplen_a >= sizeof(*sack_a) &&
 976             (!sack_b || oplen_b < sizeof(*sack_b)))
 977                 return -1;
 978         else if (sack_b && oplen_b >= sizeof(*sack_b) &&
 979                  (!sack_a || oplen_a < sizeof(*sack_a)))
 980                 return 1;
 981         else if ((!sack_a || oplen_a < sizeof(*sack_a)) &&
 982                  (!sack_b || oplen_b < sizeof(*sack_b)))
 983                 return 0;
 984
 985         while (oplen_a >= sizeof(*sack_a)) {
 986                 const struct tcp_sack_block_wire *sack_tmp = sack_b;
 987                 u32 start_a = get_unaligned_be32(&sack_a->start_seq);
 988                 u32 end_a = get_unaligned_be32(&sack_a->end_seq);
 989                 int oplen_tmp = oplen_b;
 990                 bool found = false;
 991
 992                 /* DSACK; always considered greater to prevent dropping */
 993                 if (before(start_a, ack_seq_a))
 994                         return -1;
 995
 996                 bytes_a += end_a - start_a;
 997
 998                 while (oplen_tmp >= sizeof(*sack_tmp)) {
 999                         u32 start_b = get_unaligned_be32(&sack_tmp->start_seq);
1000                         u32 end_b = get_unaligned_be32(&sack_tmp->end_seq);
1001
1002                         /* first time through we count the total size */
1003                         if (first)
1004                                 bytes_b += end_b - start_b;
1005
1006                         if (!after(start_b, start_a) && !before(end_b, end_a)) {
1007                                 found = true;
1008                                 if (!first)
1009                                         break;
1010                         }
1011                         oplen_tmp -= sizeof(*sack_tmp);
1012                         sack_tmp++;
1013                 }
1014
1015                 if (!found)
1016                         return -1;
1017
1018                 oplen_a -= sizeof(*sack_a);
1019                 sack_a++;
1020                 first = false;
1021         }
1022
1023         /* If we made it this far, all ranges SACKed by A are covered by B, so
1024          * either the SACKs are equal, or B SACKs more bytes.
1025          */
1026         return bytes_b > bytes_a ? 1 : 0;
1027 }
1028
1029 static void cake_tcph_get_tstamp(const struct tcphdr *tcph,
1030                                  u32 *tsval, u32 *tsecr)
1031 {
1032         const u8 *ptr;
1033         int opsize;
1034
1035         ptr = cake_get_tcpopt(tcph, TCPOPT_TIMESTAMP, &opsize);
1036
1037         if (ptr && opsize == TCPOLEN_TIMESTAMP) {
1038                 *tsval = get_unaligned_be32(ptr);
1039                 *tsecr = get_unaligned_be32(ptr + 4);
1040         }
1041 }
1042
1043 static bool cake_tcph_may_drop(const struct tcphdr *tcph,
1044                                u32 tstamp_new, u32 tsecr_new)
1045 {
1046         /* inspired by tcp_parse_options in tcp_input.c */
1047         int length = __tcp_hdrlen(tcph) - sizeof(struct tcphdr);
1048         const u8 *ptr = (const u8 *)(tcph + 1);
1049         u32 tstamp, tsecr;
1050
1051         /* 3 reserved flags must be unset to avoid future breakage
1052          * ACK must be set
1053          * ECE/CWR are handled separately
1054          * All other flags URG/PSH/RST/SYN/FIN must be unset
1055          * 0x0FFF0000 = all TCP flags (confirm ACK=1, others zero)
1056          * 0x00C00000 = CWR/ECE (handled separately)
1057          * 0x0F3F0000 = 0x0FFF0000 & ~0x00C00000
1058          */
1059         if (((tcp_flag_word(tcph) &
1060               cpu_to_be32(0x0F3F0000)) != TCP_FLAG_ACK))
1061                 return false;
1062
1063         while (length > 0) {
1064                 int opcode = *ptr++;
1065                 int opsize;
1066
1067                 if (opcode == TCPOPT_EOL)
1068                         break;
1069                 if (opcode == TCPOPT_NOP) {
1070                         length--;
1071                         continue;
1072                 }
1073                 if (length < 2)
1074                         break;
1075                 opsize = *ptr++;
1076                 if (opsize < 2 || opsize > length)
1077                         break;
1078
1079                 switch (opcode) {
1080                 case TCPOPT_MD5SIG: /* doesn't influence state */
1081                         break;
1082
1083                 case TCPOPT_SACK: /* stricter checking performed later */
1084                         if (opsize % 8 != 2)
1085                                 return false;
1086                         break;
1087
1088                 case TCPOPT_TIMESTAMP:
1089                         /* only drop timestamps lower than new */
1090                         if (opsize != TCPOLEN_TIMESTAMP)
1091                                 return false;
1092                         tstamp = get_unaligned_be32(ptr);
1093                         tsecr = get_unaligned_be32(ptr + 4);
1094                         if (after(tstamp, tstamp_new) ||
1095                             after(tsecr, tsecr_new))
1096                                 return false;
1097                         break;
1098
1099                 case TCPOPT_MSS:  /* these should only be set on SYN */
1100                 case TCPOPT_WINDOW:
1101                 case TCPOPT_SACK_PERM:
1102                 case TCPOPT_FASTOPEN:
1103                 case TCPOPT_EXP:
1104                 default: /* don't drop if any unknown options are present */
1105                         return false;
1106                 }
1107
1108                 ptr += opsize - 2;
1109                 length -= opsize;
1110         }
1111
1112         return true;
1113 }
1114
1115 static struct sk_buff *cake_ack_filter(struct cake_sched_data *q,
1116                                        struct cake_flow *flow)
1117 {
1118         bool aggressive = q->ack_filter == CAKE_ACK_AGGRESSIVE;
1119         struct sk_buff *elig_ack = NULL, *elig_ack_prev = NULL;
1120         struct sk_buff *skb_check, *skb_prev = NULL;
1121         const struct ipv6hdr *ipv6h, *ipv6h_check;
1122         unsigned char _tcph[64], _tcph_check[64];
1123         const struct tcphdr *tcph, *tcph_check;
1124         const struct iphdr *iph, *iph_check;
1125         struct ipv6hdr _iph, _iph_check;
1126         const struct sk_buff *skb;
1127         int seglen, num_found = 0;
1128         u32 tstamp = 0, tsecr = 0;
1129         __be32 elig_flags = 0;
1130         int sack_comp;
1131
1132         /* no other possible ACKs to filter */
1133         if (flow->head == flow->tail)
1134                 return NULL;
1135
1136         skb = flow->tail;
1137         tcph = cake_get_tcphdr(skb, _tcph, sizeof(_tcph));
1138         iph = cake_get_iphdr(skb, &_iph);
1139         if (!tcph)
1140                 return NULL;
1141
1142         cake_tcph_get_tstamp(tcph, &tstamp, &tsecr);
1143
1144         /* the 'triggering' packet need only have the ACK flag set.
1145          * also check that SYN is not set, as there won't be any previous ACKs.
1146          */
1147         if ((tcp_flag_word(tcph) &
1148              (TCP_FLAG_ACK | TCP_FLAG_SYN)) != TCP_FLAG_ACK)
1149                 return NULL;
1150
1151         /* the 'triggering' ACK is at the tail of the queue, we have already
1152          * returned if it is the only packet in the flow. loop through the rest
1153          * of the queue looking for pure ACKs with the same 5-tuple as the
1154          * triggering one.
1155          */
1156         for (skb_check = flow->head;
1157              skb_check && skb_check != skb;
1158              skb_prev = skb_check, skb_check = skb_check->next) {
1159                 iph_check = cake_get_iphdr(skb_check, &_iph_check);
1160                 tcph_check = cake_get_tcphdr(skb_check, &_tcph_check,
1161                                              sizeof(_tcph_check));
1162
1163                 /* only TCP packets with matching 5-tuple are eligible, and only
1164                  * drop safe headers
1165                  */
1166                 if (!tcph_check || iph->version != iph_check->version ||
1167                     tcph_check->source != tcph->source ||
1168                     tcph_check->dest != tcph->dest)
1169                         continue;
1170
1171                 if (iph_check->version == 4) {
1172                         if (iph_check->saddr != iph->saddr ||
1173                             iph_check->daddr != iph->daddr)
1174                                 continue;
1175
1176                         seglen = ntohs(iph_check->tot_len) -
1177                                        (4 * iph_check->ihl);
1178                 } else if (iph_check->version == 6) {
1179                         ipv6h = (struct ipv6hdr *)iph;
1180                         ipv6h_check = (struct ipv6hdr *)iph_check;
1181
1182                         if (ipv6_addr_cmp(&ipv6h_check->saddr, &ipv6h->saddr) ||
1183                             ipv6_addr_cmp(&ipv6h_check->daddr, &ipv6h->daddr))
1184                                 continue;
1185
1186                         seglen = ntohs(ipv6h_check->payload_len);
1187                 } else {
1188                         WARN_ON(1);  /* shouldn't happen */
1189                         continue;
1190                 }
1191
1192                 /* If the ECE/CWR flags changed from the previous eligible
1193                  * packet in the same flow, we should no longer be dropping that
1194                  * previous packet as this would lose information.
1195                  */
1196                 if (elig_ack && (tcp_flag_word(tcph_check) &
1197                                  (TCP_FLAG_ECE | TCP_FLAG_CWR)) != elig_flags) {
1198                         elig_ack = NULL;
1199                         elig_ack_prev = NULL;
1200                         num_found--;
1201                 }
1202
1203                 /* Check TCP options and flags, don't drop ACKs with segment
1204                  * data, and don't drop ACKs with a higher cumulative ACK
1205                  * counter than the triggering packet. Check ACK seqno here to
1206                  * avoid parsing SACK options of packets we are going to exclude
1207                  * anyway.
1208                  */
1209                 if (!cake_tcph_may_drop(tcph_check, tstamp, tsecr) ||
1210                     (seglen - __tcp_hdrlen(tcph_check)) != 0 ||
1211                     after(ntohl(tcph_check->ack_seq), ntohl(tcph->ack_seq)))
1212                         continue;
1213
1214                 /* Check SACK options. The triggering packet must SACK more data
1215                  * than the ACK under consideration, or SACK the same range but
1216                  * have a larger cumulative ACK counter. The latter is a
1217                  * pathological case, but is contained in the following check
1218                  * anyway, just to be safe.
1219                  */
1220                 sack_comp = cake_tcph_sack_compare(tcph_check, tcph);
1221
1222                 if (sack_comp < 0 ||
1223                     (ntohl(tcph_check->ack_seq) == ntohl(tcph->ack_seq) &&
1224                      sack_comp == 0))
1225                         continue;
1226
1227                 /* At this point we have found an eligible pure ACK to drop; if
1228                  * we are in aggressive mode, we are done. Otherwise, keep
1229                  * searching unless this is the second eligible ACK we
1230                  * found.
1231                  *
1232                  * Since we want to drop ACK closest to the head of the queue,
1233                  * save the first eligible ACK we find, even if we need to loop
1234                  * again.
1235                  */
1236                 if (!elig_ack) {
1237                         elig_ack = skb_check;
1238                         elig_ack_prev = skb_prev;
1239                         elig_flags = (tcp_flag_word(tcph_check)
1240                                       & (TCP_FLAG_ECE | TCP_FLAG_CWR));
1241                 }
1242
1243                 if (num_found++ > 0)
1244                         goto found;
1245         }
1246
1247         /* We made it through the queue without finding two eligible ACKs . If
1248          * we found a single eligible ACK we can drop it in aggressive mode if
1249          * we can guarantee that this does not interfere with ECN flag
1250          * information. We ensure this by dropping it only if the enqueued
1251          * packet is consecutive with the eligible ACK, and their flags match.
1252          */
1253         if (elig_ack && aggressive && elig_ack->next == skb &&
1254             (elig_flags == (tcp_flag_word(tcph) &
1255                             (TCP_FLAG_ECE | TCP_FLAG_CWR))))
1256                 goto found;
1257
1258         return NULL;
1259
1260 found:
1261         if (elig_ack_prev)
1262                 elig_ack_prev->next = elig_ack->next;
1263         else
1264                 flow->head = elig_ack->next;
1265
1266         skb_mark_not_on_list(elig_ack);
1267
1268         return elig_ack;
1269 }
1270
1271 static u64 cake_ewma(u64 avg, u64 sample, u32 shift)
1272 {
1273         avg -= avg >> shift;
1274         avg += sample >> shift;
1275         return avg;
1276 }
1277
1278 static u32 cake_calc_overhead(struct cake_sched_data *q, u32 len, u32 off)
1279 {
1280         if (q->rate_flags & CAKE_FLAG_OVERHEAD)
1281                 len -= off;
1282
1283         if (q->max_netlen < len)
1284                 q->max_netlen = len;
1285         if (q->min_netlen > len)
1286                 q->min_netlen = len;
1287
1288         len += q->rate_overhead;
1289
1290         if (len < q->rate_mpu)
1291                 len = q->rate_mpu;
1292
1293         if (q->atm_mode == CAKE_ATM_ATM) {
1294                 len += 47;
1295                 len /= 48;
1296                 len *= 53;
1297         } else if (q->atm_mode == CAKE_ATM_PTM) {
1298                 /* Add one byte per 64 bytes or part thereof.
1299                  * This is conservative and easier to calculate than the
1300                  * precise value.
1301                  */
1302                 len += (len + 63) / 64;
1303         }
1304
1305         if (q->max_adjlen < len)
1306                 q->max_adjlen = len;
1307         if (q->min_adjlen > len)
1308                 q->min_adjlen = len;
1309
1310         return len;
1311 }
1312
1313 static u32 cake_overhead(struct cake_sched_data *q, const struct sk_buff *skb)
1314 {
1315         const struct skb_shared_info *shinfo = skb_shinfo(skb);
1316         unsigned int hdr_len, last_len = 0;
1317         u32 off = skb_network_offset(skb);
1318         u32 len = qdisc_pkt_len(skb);
1319         u16 segs = 1;
1320
1321         q->avg_netoff = cake_ewma(q->avg_netoff, off << 16, 8);
1322
1323         if (!shinfo->gso_size)
1324                 return cake_calc_overhead(q, len, off);
1325
1326         /* borrowed from qdisc_pkt_len_init() */
1327         hdr_len = skb_transport_header(skb) - skb_mac_header(skb);
1328
1329         /* + transport layer */
1330         if (likely(shinfo->gso_type & (SKB_GSO_TCPV4 |
1331                                                 SKB_GSO_TCPV6))) {
1332                 const struct tcphdr *th;
1333                 struct tcphdr _tcphdr;
1334
1335                 th = skb_header_pointer(skb, skb_transport_offset(skb),
1336                                         sizeof(_tcphdr), &_tcphdr);
1337                 if (likely(th))
1338                         hdr_len += __tcp_hdrlen(th);
1339         } else {
1340                 struct udphdr _udphdr;
1341
1342                 if (skb_header_pointer(skb, skb_transport_offset(skb),
1343                                        sizeof(_udphdr), &_udphdr))
1344                         hdr_len += sizeof(struct udphdr);
1345         }
1346
1347         if (unlikely(shinfo->gso_type & SKB_GSO_DODGY))
1348                 segs = DIV_ROUND_UP(skb->len - hdr_len,
1349                                     shinfo->gso_size);
1350         else
1351                 segs = shinfo->gso_segs;
1352
1353         len = shinfo->gso_size + hdr_len;
1354         last_len = skb->len - shinfo->gso_size * (segs - 1);
1355
1356         return (cake_calc_overhead(q, len, off) * (segs - 1) +
1357                 cake_calc_overhead(q, last_len, off));
1358 }
1359
1360 static void cake_heap_swap(struct cake_sched_data *q, u16 i, u16 j)
1361 {
1362         struct cake_heap_entry ii = q->overflow_heap[i];
1363         struct cake_heap_entry jj = q->overflow_heap[j];
1364
1365         q->overflow_heap[i] = jj;
1366         q->overflow_heap[j] = ii;
1367
1368         q->tins[ii.t].overflow_idx[ii.b] = j;
1369         q->tins[jj.t].overflow_idx[jj.b] = i;
1370 }
1371
1372 static u32 cake_heap_get_backlog(const struct cake_sched_data *q, u16 i)
1373 {
1374         struct cake_heap_entry ii = q->overflow_heap[i];
1375
1376         return q->tins[ii.t].backlogs[ii.b];
1377 }
1378
1379 static void cake_heapify(struct cake_sched_data *q, u16 i)
1380 {
1381         static const u32 a = CAKE_MAX_TINS * CAKE_QUEUES;
1382         u32 mb = cake_heap_get_backlog(q, i);
1383         u32 m = i;
1384
1385         while (m < a) {
1386                 u32 l = m + m + 1;
1387                 u32 r = l + 1;
1388
1389                 if (l < a) {
1390                         u32 lb = cake_heap_get_backlog(q, l);
1391
1392                         if (lb > mb) {
1393                                 m  = l;
1394                                 mb = lb;
1395                         }
1396                 }
1397
1398                 if (r < a) {
1399                         u32 rb = cake_heap_get_backlog(q, r);
1400
1401                         if (rb > mb) {
1402                                 m  = r;
1403                                 mb = rb;
1404                         }
1405                 }
1406
1407                 if (m != i) {
1408                         cake_heap_swap(q, i, m);
1409                         i = m;
1410                 } else {
1411                         break;
1412                 }
1413         }
1414 }
1415
1416 static void cake_heapify_up(struct cake_sched_data *q, u16 i)
1417 {
1418         while (i > 0 && i < CAKE_MAX_TINS * CAKE_QUEUES) {
1419                 u16 p = (i - 1) >> 1;
1420                 u32 ib = cake_heap_get_backlog(q, i);
1421                 u32 pb = cake_heap_get_backlog(q, p);
1422
1423                 if (ib > pb) {
1424                         cake_heap_swap(q, i, p);
1425                         i = p;
1426                 } else {
1427                         break;
1428                 }
1429         }
1430 }
1431
1432 static int cake_advance_shaper(struct cake_sched_data *q,
1433                                struct cake_tin_data *b,
1434                                struct sk_buff *skb,
1435                                ktime_t now, bool drop)
1436 {
1437         u32 len = get_cobalt_cb(skb)->adjusted_len;
1438
1439         /* charge packet bandwidth to this tin
1440          * and to the global shaper.
1441          */
1442         if (q->rate_ns) {
1443                 u64 tin_dur = (len * b->tin_rate_ns) >> b->tin_rate_shft;
1444                 u64 global_dur = (len * q->rate_ns) >> q->rate_shft;
1445                 u64 failsafe_dur = global_dur + (global_dur >> 1);
1446
1447                 if (ktime_before(b->time_next_packet, now))
1448                         b->time_next_packet = ktime_add_ns(b->time_next_packet,
1449                                                            tin_dur);
1450
1451                 else if (ktime_before(b->time_next_packet,
1452                                       ktime_add_ns(now, tin_dur)))
1453                         b->time_next_packet = ktime_add_ns(now, tin_dur);
1454
1455                 q->time_next_packet = ktime_add_ns(q->time_next_packet,
1456                                                    global_dur);
1457                 if (!drop)
1458                         q->failsafe_next_packet = \
1459                                 ktime_add_ns(q->failsafe_next_packet,
1460                                              failsafe_dur);
1461         }
1462         return len;
1463 }
1464
1465 static unsigned int cake_drop(struct Qdisc *sch, struct sk_buff **to_free)
1466 {
1467         struct cake_sched_data *q = qdisc_priv(sch);
1468         ktime_t now = ktime_get();
1469         u32 idx = 0, tin = 0, len;
1470         struct cake_heap_entry qq;
1471         struct cake_tin_data *b;
1472         struct cake_flow *flow;
1473         struct sk_buff *skb;
1474
1475         if (!q->overflow_timeout) {
1476                 int i;
1477                 /* Build fresh max-heap */
1478                 for (i = CAKE_MAX_TINS * CAKE_QUEUES / 2; i >= 0; i--)
1479                         cake_heapify(q, i);
1480         }
1481         q->overflow_timeout = 65535;
1482
1483         /* select longest queue for pruning */
1484         qq  = q->overflow_heap[0];
1485         tin = qq.t;
1486         idx = qq.b;
1487
1488         b = &q->tins[tin];
1489         flow = &b->flows[idx];
1490         skb = dequeue_head(flow);
1491         if (unlikely(!skb)) {
1492                 /* heap has gone wrong, rebuild it next time */
1493                 q->overflow_timeout = 0;
1494                 return idx + (tin << 16);
1495         }
1496
1497         if (cobalt_queue_full(&flow->cvars, &b->cparams, now))
1498                 b->unresponsive_flow_count++;
1499
1500         len = qdisc_pkt_len(skb);
1501         q->buffer_used      -= skb->truesize;
1502         b->backlogs[idx]    -= len;
1503         b->tin_backlog      -= len;
1504         sch->qstats.backlog -= len;
1505         qdisc_tree_reduce_backlog(sch, 1, len);
1506
1507         flow->dropped++;
1508         b->tin_dropped++;
1509         sch->qstats.drops++;
1510
1511         if (q->rate_flags & CAKE_FLAG_INGRESS)
1512                 cake_advance_shaper(q, b, skb, now, true);
1513
1514         __qdisc_drop(skb, to_free);
1515         sch->q.qlen--;
1516
1517         cake_heapify(q, 0);
1518
1519         return idx + (tin << 16);
1520 }
1521
1522 static u8 cake_handle_diffserv(struct sk_buff *skb, bool wash)
1523 {
1524         const int offset = skb_network_offset(skb);
1525         u16 *buf, buf_;
1526         u8 dscp;
1527
1528         switch (skb_protocol(skb, true)) {
1529         case htons(ETH_P_IP):
1530                 buf = skb_header_pointer(skb, offset, sizeof(buf_), &buf_);
1531                 if (unlikely(!buf))
1532                         return 0;
1533
1534                 /* ToS is in the second byte of iphdr */
1535                 dscp = ipv4_get_dsfield((struct iphdr *)buf) >> 2;
1536
1537                 if (wash && dscp) {
1538                         const int wlen = offset + sizeof(struct iphdr);
1539
1540                         if (!pskb_may_pull(skb, wlen) ||
1541                             skb_try_make_writable(skb, wlen))
1542                                 return 0;
1543
1544                         ipv4_change_dsfield(ip_hdr(skb), INET_ECN_MASK, 0);
1545                 }
1546
1547                 return dscp;
1548
1549         case htons(ETH_P_IPV6):
1550                 buf = skb_header_pointer(skb, offset, sizeof(buf_), &buf_);
1551                 if (unlikely(!buf))
1552                         return 0;
1553
1554                 /* Traffic class is in the first and second bytes of ipv6hdr */
1555                 dscp = ipv6_get_dsfield((struct ipv6hdr *)buf) >> 2;
1556
1557                 if (wash && dscp) {
1558                         const int wlen = offset + sizeof(struct ipv6hdr);
1559
1560                         if (!pskb_may_pull(skb, wlen) ||
1561                             skb_try_make_writable(skb, wlen))
1562                                 return 0;
1563
1564                         ipv6_change_dsfield(ipv6_hdr(skb), INET_ECN_MASK, 0);
1565                 }
1566
1567                 return dscp;
1568
1569         case htons(ETH_P_ARP):
1570                 return 0x38;  /* CS7 - Net Control */
1571
1572         default:
1573                 /* If there is no Diffserv field, treat as best-effort */
1574                 return 0;
1575         }
1576 }
1577
1578 static struct cake_tin_data *cake_select_tin(struct Qdisc *sch,
1579                                              struct sk_buff *skb)
1580 {
1581         struct cake_sched_data *q = qdisc_priv(sch);
1582         u32 tin, mark;
1583         bool wash;
1584         u8 dscp;
1585
1586         /* Tin selection: Default to diffserv-based selection, allow overriding
1587          * using firewall marks or skb->priority. Call DSCP parsing early if
1588          * wash is enabled, otherwise defer to below to skip unneeded parsing.
1589          */
1590         mark = (skb->mark & q->fwmark_mask) >> q->fwmark_shft;
1591         wash = !!(q->rate_flags & CAKE_FLAG_WASH);
1592         if (wash)
1593                 dscp = cake_handle_diffserv(skb, wash);
1594
1595         if (q->tin_mode == CAKE_DIFFSERV_BESTEFFORT)
1596                 tin = 0;
1597
1598         else if (mark && mark <= q->tin_cnt)
1599                 tin = q->tin_order[mark - 1];
1600
1601         else if (TC_H_MAJ(skb->priority) == sch->handle &&
1602                  TC_H_MIN(skb->priority) > 0 &&
1603                  TC_H_MIN(skb->priority) <= q->tin_cnt)
1604                 tin = q->tin_order[TC_H_MIN(skb->priority) - 1];
1605
1606         else {
1607                 if (!wash)
1608                         dscp = cake_handle_diffserv(skb, wash);
1609                 tin = q->tin_index[dscp];
1610
1611                 if (unlikely(tin >= q->tin_cnt))
1612                         tin = 0;
1613         }
1614
1615         return &q->tins[tin];
1616 }
1617
1618 static u32 cake_classify(struct Qdisc *sch, struct cake_tin_data **t,
1619                          struct sk_buff *skb, int flow_mode, int *qerr)
1620 {
1621         struct cake_sched_data *q = qdisc_priv(sch);
1622         struct tcf_proto *filter;
1623         struct tcf_result res;
1624         u16 flow = 0, host = 0;
1625         int result;
1626
1627         filter = rcu_dereference_bh(q->filter_list);
1628         if (!filter)
1629                 goto hash;
1630
1631         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
1632         result = tcf_classify(skb, filter, &res, false);
1633
1634         if (result >= 0) {
1635 #ifdef CONFIG_NET_CLS_ACT
1636                 switch (result) {
1637                 case TC_ACT_STOLEN:
1638                 case TC_ACT_QUEUED:
1639                 case TC_ACT_TRAP:
1640                         *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
1641                         /* fall through */
1642                 case TC_ACT_SHOT:
1643                         return 0;
1644                 }
1645 #endif
1646                 if (TC_H_MIN(res.classid) <= CAKE_QUEUES)
1647                         flow = TC_H_MIN(res.classid);
1648                 if (TC_H_MAJ(res.classid) <= (CAKE_QUEUES << 16))
1649                         host = TC_H_MAJ(res.classid) >> 16;
1650         }
1651 hash:
1652         *t = cake_select_tin(sch, skb);
1653         return cake_hash(*t, skb, flow_mode, flow, host) + 1;
1654 }
1655
1656 static void cake_reconfigure(struct Qdisc *sch);
1657
1658 static s32 cake_enqueue(struct sk_buff *skb, struct Qdisc *sch,
1659                         struct sk_buff **to_free)
1660 {
1661         struct cake_sched_data *q = qdisc_priv(sch);
1662         int len = qdisc_pkt_len(skb);
1663         int ret;
1664         struct sk_buff *ack = NULL;
1665         ktime_t now = ktime_get();
1666         struct cake_tin_data *b;
1667         struct cake_flow *flow;
1668         u32 idx;
1669
1670         /* choose flow to insert into */
1671         idx = cake_classify(sch, &b, skb, q->flow_mode, &ret);
1672         if (idx == 0) {
1673                 if (ret & __NET_XMIT_BYPASS)
1674                         qdisc_qstats_drop(sch);
1675                 __qdisc_drop(skb, to_free);
1676                 return ret;
1677         }
1678         idx--;
1679         flow = &b->flows[idx];
1680
1681         /* ensure shaper state isn't stale */
1682         if (!b->tin_backlog) {
1683                 if (ktime_before(b->time_next_packet, now))
1684                         b->time_next_packet = now;
1685
1686                 if (!sch->q.qlen) {
1687                         if (ktime_before(q->time_next_packet, now)) {
1688                                 q->failsafe_next_packet = now;
1689                                 q->time_next_packet = now;
1690                         } else if (ktime_after(q->time_next_packet, now) &&
1691                                    ktime_after(q->failsafe_next_packet, now)) {
1692                                 u64 next = \
1693                                         min(ktime_to_ns(q->time_next_packet),
1694                                             ktime_to_ns(
1695                                                    q->failsafe_next_packet));
1696                                 sch->qstats.overlimits++;
1697                                 qdisc_watchdog_schedule_ns(&q->watchdog, next);
1698                         }
1699                 }
1700         }
1701
1702         if (unlikely(len > b->max_skblen))
1703                 b->max_skblen = len;
1704
1705         if (skb_is_gso(skb) && q->rate_flags & CAKE_FLAG_SPLIT_GSO) {
1706                 struct sk_buff *segs, *nskb;
1707                 netdev_features_t features = netif_skb_features(skb);
1708                 unsigned int slen = 0, numsegs = 0;
1709
1710                 segs = skb_gso_segment(skb, features & ~NETIF_F_GSO_MASK);
1711                 if (IS_ERR_OR_NULL(segs))
1712                         return qdisc_drop(skb, sch, to_free);
1713
1714                 while (segs) {
1715                         nskb = segs->next;
1716                         skb_mark_not_on_list(segs);
1717                         qdisc_skb_cb(segs)->pkt_len = segs->len;
1718                         cobalt_set_enqueue_time(segs, now);
1719                         get_cobalt_cb(segs)->adjusted_len = cake_overhead(q,
1720                                                                           segs);
1721                         flow_queue_add(flow, segs);
1722
1723                         sch->q.qlen++;
1724                         numsegs++;
1725                         slen += segs->len;
1726                         q->buffer_used += segs->truesize;
1727                         b->packets++;
1728                         segs = nskb;
1729                 }
1730
1731                 /* stats */
1732                 b->bytes            += slen;
1733                 b->backlogs[idx]    += slen;
1734                 b->tin_backlog      += slen;
1735                 sch->qstats.backlog += slen;
1736                 q->avg_window_bytes += slen;
1737
1738                 qdisc_tree_reduce_backlog(sch, 1-numsegs, len-slen);
1739                 consume_skb(skb);
1740         } else {
1741                 /* not splitting */
1742                 cobalt_set_enqueue_time(skb, now);
1743                 get_cobalt_cb(skb)->adjusted_len = cake_overhead(q, skb);
1744                 flow_queue_add(flow, skb);
1745
1746                 if (q->ack_filter)
1747                         ack = cake_ack_filter(q, flow);
1748
1749                 if (ack) {
1750                         b->ack_drops++;
1751                         sch->qstats.drops++;
1752                         b->bytes += qdisc_pkt_len(ack);
1753                         len -= qdisc_pkt_len(ack);
1754                         q->buffer_used += skb->truesize - ack->truesize;
1755                         if (q->rate_flags & CAKE_FLAG_INGRESS)
1756                                 cake_advance_shaper(q, b, ack, now, true);
1757
1758                         qdisc_tree_reduce_backlog(sch, 1, qdisc_pkt_len(ack));
1759                         consume_skb(ack);
1760                 } else {
1761                         sch->q.qlen++;
1762                         q->buffer_used      += skb->truesize;
1763                 }
1764
1765                 /* stats */
1766                 b->packets++;
1767                 b->bytes            += len;
1768                 b->backlogs[idx]    += len;
1769                 b->tin_backlog      += len;
1770                 sch->qstats.backlog += len;
1771                 q->avg_window_bytes += len;
1772         }
1773
1774         if (q->overflow_timeout)
1775                 cake_heapify_up(q, b->overflow_idx[idx]);
1776
1777         /* incoming bandwidth capacity estimate */
1778         if (q->rate_flags & CAKE_FLAG_AUTORATE_INGRESS) {
1779                 u64 packet_interval = \
1780                         ktime_to_ns(ktime_sub(now, q->last_packet_time));
1781
1782                 if (packet_interval > NSEC_PER_SEC)
1783                         packet_interval = NSEC_PER_SEC;
1784
1785                 /* filter out short-term bursts, eg. wifi aggregation */
1786                 q->avg_packet_interval = \
1787                         cake_ewma(q->avg_packet_interval,
1788                                   packet_interval,
1789                                   (packet_interval > q->avg_packet_interval ?
1790                                           2 : 8));
1791
1792                 q->last_packet_time = now;
1793
1794                 if (packet_interval > q->avg_packet_interval) {
1795                         u64 window_interval = \
1796                                 ktime_to_ns(ktime_sub(now,
1797                                                       q->avg_window_begin));
1798                         u64 b = q->avg_window_bytes * (u64)NSEC_PER_SEC;
1799
1800                         b = div64_u64(b, window_interval);
1801                         q->avg_peak_bandwidth =
1802                                 cake_ewma(q->avg_peak_bandwidth, b,
1803                                           b > q->avg_peak_bandwidth ? 2 : 8);
1804                         q->avg_window_bytes = 0;
1805                         q->avg_window_begin = now;
1806
1807                         if (ktime_after(now,
1808                                         ktime_add_ms(q->last_reconfig_time,
1809                                                      250))) {
1810                                 q->rate_bps = (q->avg_peak_bandwidth * 15) >> 4;
1811                                 cake_reconfigure(sch);
1812                         }
1813                 }
1814         } else {
1815                 q->avg_window_bytes = 0;
1816                 q->last_packet_time = now;
1817         }
1818
1819         /* flowchain */
1820         if (!flow->set || flow->set == CAKE_SET_DECAYING) {
1821                 struct cake_host *srchost = &b->hosts[flow->srchost];
1822                 struct cake_host *dsthost = &b->hosts[flow->dsthost];
1823                 u16 host_load = 1;
1824
1825                 if (!flow->set) {
1826                         list_add_tail(&flow->flowchain, &b->new_flows);
1827                 } else {
1828                         b->decaying_flow_count--;
1829                         list_move_tail(&flow->flowchain, &b->new_flows);
1830                 }
1831                 flow->set = CAKE_SET_SPARSE;
1832                 b->sparse_flow_count++;
1833
1834                 if (cake_dsrc(q->flow_mode))
1835                         host_load = max(host_load, srchost->srchost_bulk_flow_count);
1836
1837                 if (cake_ddst(q->flow_mode))
1838                         host_load = max(host_load, dsthost->dsthost_bulk_flow_count);
1839
1840                 flow->deficit = (b->flow_quantum *
1841                                  quantum_div[host_load]) >> 16;
1842         } else if (flow->set == CAKE_SET_SPARSE_WAIT) {
1843                 struct cake_host *srchost = &b->hosts[flow->srchost];
1844                 struct cake_host *dsthost = &b->hosts[flow->dsthost];
1845
1846                 /* this flow was empty, accounted as a sparse flow, but actually
1847                  * in the bulk rotation.
1848                  */
1849                 flow->set = CAKE_SET_BULK;
1850                 b->sparse_flow_count--;
1851                 b->bulk_flow_count++;
1852
1853                 if (cake_dsrc(q->flow_mode))
1854                         srchost->srchost_bulk_flow_count++;
1855
1856                 if (cake_ddst(q->flow_mode))
1857                         dsthost->dsthost_bulk_flow_count++;
1858
1859         }
1860
1861         if (q->buffer_used > q->buffer_max_used)
1862                 q->buffer_max_used = q->buffer_used;
1863
1864         if (q->buffer_used > q->buffer_limit) {
1865                 u32 dropped = 0;
1866
1867                 while (q->buffer_used > q->buffer_limit) {
1868                         dropped++;
1869                         cake_drop(sch, to_free);
1870                 }
1871                 b->drop_overlimit += dropped;
1872         }
1873         return NET_XMIT_SUCCESS;
1874 }
1875
1876 static struct sk_buff *cake_dequeue_one(struct Qdisc *sch)
1877 {
1878         struct cake_sched_data *q = qdisc_priv(sch);
1879         struct cake_tin_data *b = &q->tins[q->cur_tin];
1880         struct cake_flow *flow = &b->flows[q->cur_flow];
1881         struct sk_buff *skb = NULL;
1882         u32 len;
1883
1884         if (flow->head) {
1885                 skb = dequeue_head(flow);
1886                 len = qdisc_pkt_len(skb);
1887                 b->backlogs[q->cur_flow] -= len;
1888                 b->tin_backlog           -= len;
1889                 sch->qstats.backlog      -= len;
1890                 q->buffer_used           -= skb->truesize;
1891                 sch->q.qlen--;
1892
1893                 if (q->overflow_timeout)
1894                         cake_heapify(q, b->overflow_idx[q->cur_flow]);
1895         }
1896         return skb;
1897 }
1898
1899 /* Discard leftover packets from a tin no longer in use. */
1900 static void cake_clear_tin(struct Qdisc *sch, u16 tin)
1901 {
1902         struct cake_sched_data *q = qdisc_priv(sch);
1903         struct sk_buff *skb;
1904
1905         q->cur_tin = tin;
1906         for (q->cur_flow = 0; q->cur_flow < CAKE_QUEUES; q->cur_flow++)
1907                 while (!!(skb = cake_dequeue_one(sch)))
1908                         kfree_skb(skb);
1909 }
1910
1911 static struct sk_buff *cake_dequeue(struct Qdisc *sch)
1912 {
1913         struct cake_sched_data *q = qdisc_priv(sch);
1914         struct cake_tin_data *b = &q->tins[q->cur_tin];
1915         struct cake_host *srchost, *dsthost;
1916         ktime_t now = ktime_get();
1917         struct cake_flow *flow;
1918         struct list_head *head;
1919         bool first_flow = true;
1920         struct sk_buff *skb;
1921         u16 host_load;
1922         u64 delay;
1923         u32 len;
1924
1925 begin:
1926         if (!sch->q.qlen)
1927                 return NULL;
1928
1929         /* global hard shaper */
1930         if (ktime_after(q->time_next_packet, now) &&
1931             ktime_after(q->failsafe_next_packet, now)) {
1932                 u64 next = min(ktime_to_ns(q->time_next_packet),
1933                                ktime_to_ns(q->failsafe_next_packet));
1934
1935                 sch->qstats.overlimits++;
1936                 qdisc_watchdog_schedule_ns(&q->watchdog, next);
1937                 return NULL;
1938         }
1939
1940         /* Choose a class to work on. */
1941         if (!q->rate_ns) {
1942                 /* In unlimited mode, can't rely on shaper timings, just balance
1943                  * with DRR
1944                  */
1945                 bool wrapped = false, empty = true;
1946
1947                 while (b->tin_deficit < 0 ||
1948                        !(b->sparse_flow_count + b->bulk_flow_count)) {
1949                         if (b->tin_deficit <= 0)
1950                                 b->tin_deficit += b->tin_quantum_band;
1951                         if (b->sparse_flow_count + b->bulk_flow_count)
1952                                 empty = false;
1953
1954                         q->cur_tin++;
1955                         b++;
1956                         if (q->cur_tin >= q->tin_cnt) {
1957                                 q->cur_tin = 0;
1958                                 b = q->tins;
1959
1960                                 if (wrapped) {
1961                                         /* It's possible for q->qlen to be
1962                                          * nonzero when we actually have no
1963                                          * packets anywhere.
1964                                          */
1965                                         if (empty)
1966                                                 return NULL;
1967                                 } else {
1968                                         wrapped = true;
1969                                 }
1970                         }
1971                 }
1972         } else {
1973                 /* In shaped mode, choose:
1974                  * - Highest-priority tin with queue and meeting schedule, or
1975                  * - The earliest-scheduled tin with queue.
1976                  */
1977                 ktime_t best_time = KTIME_MAX;
1978                 int tin, best_tin = 0;
1979
1980                 for (tin = 0; tin < q->tin_cnt; tin++) {
1981                         b = q->tins + tin;
1982                         if ((b->sparse_flow_count + b->bulk_flow_count) > 0) {
1983                                 ktime_t time_to_pkt = \
1984                                         ktime_sub(b->time_next_packet, now);
1985
1986                                 if (ktime_to_ns(time_to_pkt) <= 0 ||
1987                                     ktime_compare(time_to_pkt,
1988                                                   best_time) <= 0) {
1989                                         best_time = time_to_pkt;
1990                                         best_tin = tin;
1991                                 }
1992                         }
1993                 }
1994
1995                 q->cur_tin = best_tin;
1996                 b = q->tins + best_tin;
1997
1998                 /* No point in going further if no packets to deliver. */
1999                 if (unlikely(!(b->sparse_flow_count + b->bulk_flow_count)))
2000                         return NULL;
2001         }
2002
2003 retry:
2004         /* service this class */
2005         head = &b->decaying_flows;
2006         if (!first_flow || list_empty(head)) {
2007                 head = &b->new_flows;
2008                 if (list_empty(head)) {
2009                         head = &b->old_flows;
2010                         if (unlikely(list_empty(head))) {
2011                                 head = &b->decaying_flows;
2012                                 if (unlikely(list_empty(head)))
2013                                         goto begin;
2014                         }
2015                 }
2016         }
2017         flow = list_first_entry(head, struct cake_flow, flowchain);
2018         q->cur_flow = flow - b->flows;
2019         first_flow = false;
2020
2021         /* triple isolation (modified DRR++) */
2022         srchost = &b->hosts[flow->srchost];
2023         dsthost = &b->hosts[flow->dsthost];
2024         host_load = 1;
2025
2026         /* flow isolation (DRR++) */
2027         if (flow->deficit <= 0) {
2028                 /* Keep all flows with deficits out of the sparse and decaying
2029                  * rotations.  No non-empty flow can go into the decaying
2030                  * rotation, so they can't get deficits
2031                  */
2032                 if (flow->set == CAKE_SET_SPARSE) {
2033                         if (flow->head) {
2034                                 b->sparse_flow_count--;
2035                                 b->bulk_flow_count++;
2036
2037                                 if (cake_dsrc(q->flow_mode))
2038                                         srchost->srchost_bulk_flow_count++;
2039
2040                                 if (cake_ddst(q->flow_mode))
2041                                         dsthost->dsthost_bulk_flow_count++;
2042
2043                                 flow->set = CAKE_SET_BULK;
2044                         } else {
2045                                 /* we've moved it to the bulk rotation for
2046                                  * correct deficit accounting but we still want
2047                                  * to count it as a sparse flow, not a bulk one.
2048                                  */
2049                                 flow->set = CAKE_SET_SPARSE_WAIT;
2050                         }
2051                 }
2052
2053                 if (cake_dsrc(q->flow_mode))
2054                         host_load = max(host_load, srchost->srchost_bulk_flow_count);
2055
2056                 if (cake_ddst(q->flow_mode))
2057                         host_load = max(host_load, dsthost->dsthost_bulk_flow_count);
2058
2059                 WARN_ON(host_load > CAKE_QUEUES);
2060
2061                 /* The shifted prandom_u32() is a way to apply dithering to
2062                  * avoid accumulating roundoff errors
2063                  */
2064                 flow->deficit += (b->flow_quantum * quantum_div[host_load] +
2065                                   (prandom_u32() >> 16)) >> 16;
2066                 list_move_tail(&flow->flowchain, &b->old_flows);
2067
2068                 goto retry;
2069         }
2070
2071         /* Retrieve a packet via the AQM */
2072         while (1) {
2073                 skb = cake_dequeue_one(sch);
2074                 if (!skb) {
2075                         /* this queue was actually empty */
2076                         if (cobalt_queue_empty(&flow->cvars, &b->cparams, now))
2077                                 b->unresponsive_flow_count--;
2078
2079                         if (flow->cvars.p_drop || flow->cvars.count ||
2080                             ktime_before(now, flow->cvars.drop_next)) {
2081                                 /* keep in the flowchain until the state has
2082                                  * decayed to rest
2083                                  */
2084                                 list_move_tail(&flow->flowchain,
2085                                                &b->decaying_flows);
2086                                 if (flow->set == CAKE_SET_BULK) {
2087                                         b->bulk_flow_count--;
2088
2089                                         if (cake_dsrc(q->flow_mode))
2090                                                 srchost->srchost_bulk_flow_count--;
2091
2092                                         if (cake_ddst(q->flow_mode))
2093                                                 dsthost->dsthost_bulk_flow_count--;
2094
2095                                         b->decaying_flow_count++;
2096                                 } else if (flow->set == CAKE_SET_SPARSE ||
2097                                            flow->set == CAKE_SET_SPARSE_WAIT) {
2098                                         b->sparse_flow_count--;
2099                                         b->decaying_flow_count++;
2100                                 }
2101                                 flow->set = CAKE_SET_DECAYING;
2102                         } else {
2103                                 /* remove empty queue from the flowchain */
2104                                 list_del_init(&flow->flowchain);
2105                                 if (flow->set == CAKE_SET_SPARSE ||
2106                                     flow->set == CAKE_SET_SPARSE_WAIT)
2107                                         b->sparse_flow_count--;
2108                                 else if (flow->set == CAKE_SET_BULK) {
2109                                         b->bulk_flow_count--;
2110
2111                                         if (cake_dsrc(q->flow_mode))
2112                                                 srchost->srchost_bulk_flow_count--;
2113
2114                                         if (cake_ddst(q->flow_mode))
2115                                                 dsthost->dsthost_bulk_flow_count--;
2116
2117                                 } else
2118                                         b->decaying_flow_count--;
2119
2120                                 flow->set = CAKE_SET_NONE;
2121                         }
2122                         goto begin;
2123                 }
2124
2125                 /* Last packet in queue may be marked, shouldn't be dropped */
2126                 if (!cobalt_should_drop(&flow->cvars, &b->cparams, now, skb,
2127                                         (b->bulk_flow_count *
2128                                          !!(q->rate_flags &
2129                                             CAKE_FLAG_INGRESS))) ||
2130                     !flow->head)
2131                         break;
2132
2133                 /* drop this packet, get another one */
2134                 if (q->rate_flags & CAKE_FLAG_INGRESS) {
2135                         len = cake_advance_shaper(q, b, skb,
2136                                                   now, true);
2137                         flow->deficit -= len;
2138                         b->tin_deficit -= len;
2139                 }
2140                 flow->dropped++;
2141                 b->tin_dropped++;
2142                 qdisc_tree_reduce_backlog(sch, 1, qdisc_pkt_len(skb));
2143                 qdisc_qstats_drop(sch);
2144                 kfree_skb(skb);
2145                 if (q->rate_flags & CAKE_FLAG_INGRESS)
2146                         goto retry;
2147         }
2148
2149         b->tin_ecn_mark += !!flow->cvars.ecn_marked;
2150         qdisc_bstats_update(sch, skb);
2151
2152         /* collect delay stats */
2153         delay = ktime_to_ns(ktime_sub(now, cobalt_get_enqueue_time(skb)));
2154         b->avge_delay = cake_ewma(b->avge_delay, delay, 8);
2155         b->peak_delay = cake_ewma(b->peak_delay, delay,
2156                                   delay > b->peak_delay ? 2 : 8);
2157         b->base_delay = cake_ewma(b->base_delay, delay,
2158                                   delay < b->base_delay ? 2 : 8);
2159
2160         len = cake_advance_shaper(q, b, skb, now, false);
2161         flow->deficit -= len;
2162         b->tin_deficit -= len;
2163
2164         if (ktime_after(q->time_next_packet, now) && sch->q.qlen) {
2165                 u64 next = min(ktime_to_ns(q->time_next_packet),
2166                                ktime_to_ns(q->failsafe_next_packet));
2167
2168                 qdisc_watchdog_schedule_ns(&q->watchdog, next);
2169         } else if (!sch->q.qlen) {
2170                 int i;
2171
2172                 for (i = 0; i < q->tin_cnt; i++) {
2173                         if (q->tins[i].decaying_flow_count) {
2174                                 ktime_t next = \
2175                                         ktime_add_ns(now,
2176                                                      q->tins[i].cparams.target);
2177
2178                                 qdisc_watchdog_schedule_ns(&q->watchdog,
2179                                                            ktime_to_ns(next));
2180                                 break;
2181                         }
2182                 }
2183         }
2184
2185         if (q->overflow_timeout)
2186                 q->overflow_timeout--;
2187
2188         return skb;
2189 }
2190
2191 static void cake_reset(struct Qdisc *sch)
2192 {
2193         struct cake_sched_data *q = qdisc_priv(sch);
2194         u32 c;
2195
2196         if (!q->tins)
2197                 return;
2198
2199         for (c = 0; c < CAKE_MAX_TINS; c++)
2200                 cake_clear_tin(sch, c);
2201 }
2202
2203 static const struct nla_policy cake_policy[TCA_CAKE_MAX + 1] = {
2204         [TCA_CAKE_BASE_RATE64]   = { .type = NLA_U64 },
2205         [TCA_CAKE_DIFFSERV_MODE] = { .type = NLA_U32 },
2206         [TCA_CAKE_ATM]           = { .type = NLA_U32 },
2207         [TCA_CAKE_FLOW_MODE]     = { .type = NLA_U32 },
2208         [TCA_CAKE_OVERHEAD]      = { .type = NLA_S32 },
2209         [TCA_CAKE_RTT]           = { .type = NLA_U32 },
2210         [TCA_CAKE_TARGET]        = { .type = NLA_U32 },
2211         [TCA_CAKE_AUTORATE]      = { .type = NLA_U32 },
2212         [TCA_CAKE_MEMORY]        = { .type = NLA_U32 },
2213         [TCA_CAKE_NAT]           = { .type = NLA_U32 },
2214         [TCA_CAKE_RAW]           = { .type = NLA_U32 },
2215         [TCA_CAKE_WASH]          = { .type = NLA_U32 },
2216         [TCA_CAKE_MPU]           = { .type = NLA_U32 },
2217         [TCA_CAKE_INGRESS]       = { .type = NLA_U32 },
2218         [TCA_CAKE_ACK_FILTER]    = { .type = NLA_U32 },
2219         [TCA_CAKE_SPLIT_GSO]     = { .type = NLA_U32 },
2220         [TCA_CAKE_FWMARK]        = { .type = NLA_U32 },
2221 };
2222
2223 static void cake_set_rate(struct cake_tin_data *b, u64 rate, u32 mtu,
2224                           u64 target_ns, u64 rtt_est_ns)
2225 {
2226         /* convert byte-rate into time-per-byte
2227          * so it will always unwedge in reasonable time.
2228          */
2229         static const u64 MIN_RATE = 64;
2230         u32 byte_target = mtu;
2231         u64 byte_target_ns;
2232         u8  rate_shft = 0;
2233         u64 rate_ns = 0;
2234
2235         b->flow_quantum = 1514;
2236         if (rate) {
2237                 b->flow_quantum = max(min(rate >> 12, 1514ULL), 300ULL);
2238                 rate_shft = 34;
2239                 rate_ns = ((u64)NSEC_PER_SEC) << rate_shft;
2240                 rate_ns = div64_u64(rate_ns, max(MIN_RATE, rate));
2241                 while (!!(rate_ns >> 34)) {
2242                         rate_ns >>= 1;
2243                         rate_shft--;
2244                 }
2245         } /* else unlimited, ie. zero delay */
2246
2247         b->tin_rate_bps  = rate;
2248         b->tin_rate_ns   = rate_ns;
2249         b->tin_rate_shft = rate_shft;
2250
2251         byte_target_ns = (byte_target * rate_ns) >> rate_shft;
2252
2253         b->cparams.target = max((byte_target_ns * 3) / 2, target_ns);
2254         b->cparams.interval = max(rtt_est_ns +
2255                                      b->cparams.target - target_ns,
2256                                      b->cparams.target * 2);
2257         b->cparams.mtu_time = byte_target_ns;
2258         b->cparams.p_inc = 1 << 24; /* 1/256 */
2259         b->cparams.p_dec = 1 << 20; /* 1/4096 */
2260 }
2261
2262 static int cake_config_besteffort(struct Qdisc *sch)
2263 {
2264         struct cake_sched_data *q = qdisc_priv(sch);
2265         struct cake_tin_data *b = &q->tins[0];
2266         u32 mtu = psched_mtu(qdisc_dev(sch));
2267         u64 rate = q->rate_bps;
2268
2269         q->tin_cnt = 1;
2270
2271         q->tin_index = besteffort;
2272         q->tin_order = normal_order;
2273
2274         cake_set_rate(b, rate, mtu,
2275                       us_to_ns(q->target), us_to_ns(q->interval));
2276         b->tin_quantum_band = 65535;
2277         b->tin_quantum_prio = 65535;
2278
2279         return 0;
2280 }
2281
2282 static int cake_config_precedence(struct Qdisc *sch)
2283 {
2284         /* convert high-level (user visible) parameters into internal format */
2285         struct cake_sched_data *q = qdisc_priv(sch);
2286         u32 mtu = psched_mtu(qdisc_dev(sch));
2287         u64 rate = q->rate_bps;
2288         u32 quantum1 = 256;
2289         u32 quantum2 = 256;
2290         u32 i;
2291
2292         q->tin_cnt = 8;
2293         q->tin_index = precedence;
2294         q->tin_order = normal_order;
2295
2296         for (i = 0; i < q->tin_cnt; i++) {
2297                 struct cake_tin_data *b = &q->tins[i];
2298
2299                 cake_set_rate(b, rate, mtu, us_to_ns(q->target),
2300                               us_to_ns(q->interval));
2301
2302                 b->tin_quantum_prio = max_t(u16, 1U, quantum1);
2303                 b->tin_quantum_band = max_t(u16, 1U, quantum2);
2304
2305                 /* calculate next class's parameters */
2306                 rate  *= 7;
2307                 rate >>= 3;
2308
2309                 quantum1  *= 3;
2310                 quantum1 >>= 1;
2311
2312                 quantum2  *= 7;
2313                 quantum2 >>= 3;
2314         }
2315
2316         return 0;
2317 }
2318
2319 /*      List of known Diffserv codepoints:
2320  *
2321  *      Least Effort (CS1)
2322  *      Best Effort (CS0)
2323  *      Max Reliability & LLT "Lo" (TOS1)
2324  *      Max Throughput (TOS2)
2325  *      Min Delay (TOS4)
2326  *      LLT "La" (TOS5)
2327  *      Assured Forwarding 1 (AF1x) - x3
2328  *      Assured Forwarding 2 (AF2x) - x3
2329  *      Assured Forwarding 3 (AF3x) - x3
2330  *      Assured Forwarding 4 (AF4x) - x3
2331  *      Precedence Class 2 (CS2)
2332  *      Precedence Class 3 (CS3)
2333  *      Precedence Class 4 (CS4)
2334  *      Precedence Class 5 (CS5)
2335  *      Precedence Class 6 (CS6)
2336  *      Precedence Class 7 (CS7)
2337  *      Voice Admit (VA)
2338  *      Expedited Forwarding (EF)
2339
2340  *      Total 25 codepoints.
2341  */
2342
2343 /*      List of traffic classes in RFC 4594:
2344  *              (roughly descending order of contended priority)
2345  *              (roughly ascending order of uncontended throughput)
2346  *
2347  *      Network Control (CS6,CS7)      - routing traffic
2348  *      Telephony (EF,VA)         - aka. VoIP streams
2349  *      Signalling (CS5)               - VoIP setup
2350  *      Multimedia Conferencing (AF4x) - aka. video calls
2351  *      Realtime Interactive (CS4)     - eg. games
2352  *      Multimedia Streaming (AF3x)    - eg. YouTube, NetFlix, Twitch
2353  *      Broadcast Video (CS3)
2354  *      Low Latency Data (AF2x,TOS4)      - eg. database
2355  *      Ops, Admin, Management (CS2,TOS1) - eg. ssh
2356  *      Standard Service (CS0 & unrecognised codepoints)
2357  *      High Throughput Data (AF1x,TOS2)  - eg. web traffic
2358  *      Low Priority Data (CS1)           - eg. BitTorrent
2359
2360  *      Total 12 traffic classes.
2361  */
2362
2363 static int cake_config_diffserv8(struct Qdisc *sch)
2364 {
2365 /*      Pruned list of traffic classes for typical applications:
2366  *
2367  *              Network Control          (CS6, CS7)
2368  *              Minimum Latency          (EF, VA, CS5, CS4)
2369  *              Interactive Shell        (CS2, TOS1)
2370  *              Low Latency Transactions (AF2x, TOS4)
2371  *              Video Streaming          (AF4x, AF3x, CS3)
2372  *              Bog Standard             (CS0 etc.)
2373  *              High Throughput          (AF1x, TOS2)
2374  *              Background Traffic       (CS1)
2375  *
2376  *              Total 8 traffic classes.
2377  */
2378
2379         struct cake_sched_data *q = qdisc_priv(sch);
2380         u32 mtu = psched_mtu(qdisc_dev(sch));
2381         u64 rate = q->rate_bps;
2382         u32 quantum1 = 256;
2383         u32 quantum2 = 256;
2384         u32 i;
2385
2386         q->tin_cnt = 8;
2387
2388         /* codepoint to class mapping */
2389         q->tin_index = diffserv8;
2390         q->tin_order = normal_order;
2391
2392         /* class characteristics */
2393         for (i = 0; i < q->tin_cnt; i++) {
2394                 struct cake_tin_data *b = &q->tins[i];
2395
2396                 cake_set_rate(b, rate, mtu, us_to_ns(q->target),
2397                               us_to_ns(q->interval));
2398
2399                 b->tin_quantum_prio = max_t(u16, 1U, quantum1);
2400                 b->tin_quantum_band = max_t(u16, 1U, quantum2);
2401
2402                 /* calculate next class's parameters */
2403                 rate  *= 7;
2404                 rate >>= 3;
2405
2406                 quantum1  *= 3;
2407                 quantum1 >>= 1;
2408
2409                 quantum2  *= 7;
2410                 quantum2 >>= 3;
2411         }
2412
2413         return 0;
2414 }
2415
2416 static int cake_config_diffserv4(struct Qdisc *sch)
2417 {
2418 /*  Further pruned list of traffic classes for four-class system:
2419  *
2420  *          Latency Sensitive  (CS7, CS6, EF, VA, CS5, CS4)
2421  *          Streaming Media    (AF4x, AF3x, CS3, AF2x, TOS4, CS2, TOS1)
2422  *          Best Effort        (CS0, AF1x, TOS2, and those not specified)
2423  *          Background Traffic (CS1)
2424  *
2425  *              Total 4 traffic classes.
2426  */
2427
2428         struct cake_sched_data *q = qdisc_priv(sch);
2429         u32 mtu = psched_mtu(qdisc_dev(sch));
2430         u64 rate = q->rate_bps;
2431         u32 quantum = 1024;
2432
2433         q->tin_cnt = 4;
2434
2435         /* codepoint to class mapping */
2436         q->tin_index = diffserv4;
2437         q->tin_order = bulk_order;
2438
2439         /* class characteristics */
2440         cake_set_rate(&q->tins[0], rate, mtu,
2441                       us_to_ns(q->target), us_to_ns(q->interval));
2442         cake_set_rate(&q->tins[1], rate >> 4, mtu,
2443                       us_to_ns(q->target), us_to_ns(q->interval));
2444         cake_set_rate(&q->tins[2], rate >> 1, mtu,
2445                       us_to_ns(q->target), us_to_ns(q->interval));
2446         cake_set_rate(&q->tins[3], rate >> 2, mtu,
2447                       us_to_ns(q->target), us_to_ns(q->interval));
2448
2449         /* priority weights */
2450         q->tins[0].tin_quantum_prio = quantum;
2451         q->tins[1].tin_quantum_prio = quantum >> 4;
2452         q->tins[2].tin_quantum_prio = quantum << 2;
2453         q->tins[3].tin_quantum_prio = quantum << 4;
2454
2455         /* bandwidth-sharing weights */
2456         q->tins[0].tin_quantum_band = quantum;
2457         q->tins[1].tin_quantum_band = quantum >> 4;
2458         q->tins[2].tin_quantum_band = quantum >> 1;
2459         q->tins[3].tin_quantum_band = quantum >> 2;
2460
2461         return 0;
2462 }
2463
2464 static int cake_config_diffserv3(struct Qdisc *sch)
2465 {
2466 /*  Simplified Diffserv structure with 3 tins.
2467  *              Low Priority            (CS1)
2468  *              Best Effort
2469  *              Latency Sensitive       (TOS4, VA, EF, CS6, CS7)
2470  */
2471         struct cake_sched_data *q = qdisc_priv(sch);
2472         u32 mtu = psched_mtu(qdisc_dev(sch));
2473         u64 rate = q->rate_bps;
2474         u32 quantum = 1024;
2475
2476         q->tin_cnt = 3;
2477
2478         /* codepoint to class mapping */
2479         q->tin_index = diffserv3;
2480         q->tin_order = bulk_order;
2481
2482         /* class characteristics */
2483         cake_set_rate(&q->tins[0], rate, mtu,
2484                       us_to_ns(q->target), us_to_ns(q->interval));
2485         cake_set_rate(&q->tins[1], rate >> 4, mtu,
2486                       us_to_ns(q->target), us_to_ns(q->interval));
2487         cake_set_rate(&q->tins[2], rate >> 2, mtu,
2488                       us_to_ns(q->target), us_to_ns(q->interval));
2489
2490         /* priority weights */
2491         q->tins[0].tin_quantum_prio = quantum;
2492         q->tins[1].tin_quantum_prio = quantum >> 4;
2493         q->tins[2].tin_quantum_prio = quantum << 4;
2494
2495         /* bandwidth-sharing weights */
2496         q->tins[0].tin_quantum_band = quantum;
2497         q->tins[1].tin_quantum_band = quantum >> 4;
2498         q->tins[2].tin_quantum_band = quantum >> 2;
2499
2500         return 0;
2501 }
2502
2503 static void cake_reconfigure(struct Qdisc *sch)
2504 {
2505         struct cake_sched_data *q = qdisc_priv(sch);
2506         int c, ft;
2507
2508         switch (q->tin_mode) {
2509         case CAKE_DIFFSERV_BESTEFFORT:
2510                 ft = cake_config_besteffort(sch);
2511                 break;
2512
2513         case CAKE_DIFFSERV_PRECEDENCE:
2514                 ft = cake_config_precedence(sch);
2515                 break;
2516
2517         case CAKE_DIFFSERV_DIFFSERV8:
2518                 ft = cake_config_diffserv8(sch);
2519                 break;
2520
2521         case CAKE_DIFFSERV_DIFFSERV4:
2522                 ft = cake_config_diffserv4(sch);
2523                 break;
2524
2525         case CAKE_DIFFSERV_DIFFSERV3:
2526         default:
2527                 ft = cake_config_diffserv3(sch);
2528                 break;
2529         }
2530
2531         for (c = q->tin_cnt; c < CAKE_MAX_TINS; c++) {
2532                 cake_clear_tin(sch, c);
2533                 q->tins[c].cparams.mtu_time = q->tins[ft].cparams.mtu_time;
2534         }
2535
2536         q->rate_ns   = q->tins[ft].tin_rate_ns;
2537         q->rate_shft = q->tins[ft].tin_rate_shft;
2538
2539         if (q->buffer_config_limit) {
2540                 q->buffer_limit = q->buffer_config_limit;
2541         } else if (q->rate_bps) {
2542                 u64 t = q->rate_bps * q->interval;
2543
2544                 do_div(t, USEC_PER_SEC / 4);
2545                 q->buffer_limit = max_t(u32, t, 4U << 20);
2546         } else {
2547                 q->buffer_limit = ~0;
2548         }
2549
2550         sch->flags &= ~TCQ_F_CAN_BYPASS;
2551
2552         q->buffer_limit = min(q->buffer_limit,
2553                               max(sch->limit * psched_mtu(qdisc_dev(sch)),
2554                                   q->buffer_config_limit));
2555 }
2556
2557 static int cake_change(struct Qdisc *sch, struct nlattr *opt,
2558                        struct netlink_ext_ack *extack)
2559 {
2560         struct cake_sched_data *q = qdisc_priv(sch);
2561         struct nlattr *tb[TCA_CAKE_MAX + 1];
2562         int err;
2563
2564         if (!opt)
2565                 return -EINVAL;
2566
2567         err = nla_parse_nested_deprecated(tb, TCA_CAKE_MAX, opt, cake_policy,
2568                                           extack);
2569         if (err < 0)
2570                 return err;
2571
2572         if (tb[TCA_CAKE_NAT]) {
2573 #if IS_ENABLED(CONFIG_NF_CONNTRACK)
2574                 q->flow_mode &= ~CAKE_FLOW_NAT_FLAG;
2575                 q->flow_mode |= CAKE_FLOW_NAT_FLAG *
2576                         !!nla_get_u32(tb[TCA_CAKE_NAT]);
2577 #else
2578                 NL_SET_ERR_MSG_ATTR(extack, tb[TCA_CAKE_NAT],
2579                                     "No conntrack support in kernel");
2580                 return -EOPNOTSUPP;
2581 #endif
2582         }
2583
2584         if (tb[TCA_CAKE_BASE_RATE64])
2585                 q->rate_bps = nla_get_u64(tb[TCA_CAKE_BASE_RATE64]);
2586
2587         if (tb[TCA_CAKE_DIFFSERV_MODE])
2588                 q->tin_mode = nla_get_u32(tb[TCA_CAKE_DIFFSERV_MODE]);
2589
2590         if (tb[TCA_CAKE_WASH]) {
2591                 if (!!nla_get_u32(tb[TCA_CAKE_WASH]))
2592                         q->rate_flags |= CAKE_FLAG_WASH;
2593                 else
2594                         q->rate_flags &= ~CAKE_FLAG_WASH;
2595         }
2596
2597         if (tb[TCA_CAKE_FLOW_MODE])
2598                 q->flow_mode = ((q->flow_mode & CAKE_FLOW_NAT_FLAG) |
2599                                 (nla_get_u32(tb[TCA_CAKE_FLOW_MODE]) &
2600                                         CAKE_FLOW_MASK));
2601
2602         if (tb[TCA_CAKE_ATM])
2603                 q->atm_mode = nla_get_u32(tb[TCA_CAKE_ATM]);
2604
2605         if (tb[TCA_CAKE_OVERHEAD]) {
2606                 q->rate_overhead = nla_get_s32(tb[TCA_CAKE_OVERHEAD]);
2607                 q->rate_flags |= CAKE_FLAG_OVERHEAD;
2608
2609                 q->max_netlen = 0;
2610                 q->max_adjlen = 0;
2611                 q->min_netlen = ~0;
2612                 q->min_adjlen = ~0;
2613         }
2614
2615         if (tb[TCA_CAKE_RAW]) {
2616                 q->rate_flags &= ~CAKE_FLAG_OVERHEAD;
2617
2618                 q->max_netlen = 0;
2619                 q->max_adjlen = 0;
2620                 q->min_netlen = ~0;
2621                 q->min_adjlen = ~0;
2622         }
2623
2624         if (tb[TCA_CAKE_MPU])
2625                 q->rate_mpu = nla_get_u32(tb[TCA_CAKE_MPU]);
2626
2627         if (tb[TCA_CAKE_RTT]) {
2628                 q->interval = nla_get_u32(tb[TCA_CAKE_RTT]);
2629
2630                 if (!q->interval)
2631                         q->interval = 1;
2632         }
2633
2634         if (tb[TCA_CAKE_TARGET]) {
2635                 q->target = nla_get_u32(tb[TCA_CAKE_TARGET]);
2636
2637                 if (!q->target)
2638                         q->target = 1;
2639         }
2640
2641         if (tb[TCA_CAKE_AUTORATE]) {
2642                 if (!!nla_get_u32(tb[TCA_CAKE_AUTORATE]))
2643                         q->rate_flags |= CAKE_FLAG_AUTORATE_INGRESS;
2644                 else
2645                         q->rate_flags &= ~CAKE_FLAG_AUTORATE_INGRESS;
2646         }
2647
2648         if (tb[TCA_CAKE_INGRESS]) {
2649                 if (!!nla_get_u32(tb[TCA_CAKE_INGRESS]))
2650                         q->rate_flags |= CAKE_FLAG_INGRESS;
2651                 else
2652                         q->rate_flags &= ~CAKE_FLAG_INGRESS;
2653         }
2654
2655         if (tb[TCA_CAKE_ACK_FILTER])
2656                 q->ack_filter = nla_get_u32(tb[TCA_CAKE_ACK_FILTER]);
2657
2658         if (tb[TCA_CAKE_MEMORY])
2659                 q->buffer_config_limit = nla_get_u32(tb[TCA_CAKE_MEMORY]);
2660
2661         if (tb[TCA_CAKE_SPLIT_GSO]) {
2662                 if (!!nla_get_u32(tb[TCA_CAKE_SPLIT_GSO]))
2663                         q->rate_flags |= CAKE_FLAG_SPLIT_GSO;
2664                 else
2665                         q->rate_flags &= ~CAKE_FLAG_SPLIT_GSO;
2666         }
2667
2668         if (tb[TCA_CAKE_FWMARK]) {
2669                 q->fwmark_mask = nla_get_u32(tb[TCA_CAKE_FWMARK]);
2670                 q->fwmark_shft = q->fwmark_mask ? __ffs(q->fwmark_mask) : 0;
2671         }
2672
2673         if (q->tins) {
2674                 sch_tree_lock(sch);
2675                 cake_reconfigure(sch);
2676                 sch_tree_unlock(sch);
2677         }
2678
2679         return 0;
2680 }
2681
2682 static void cake_destroy(struct Qdisc *sch)
2683 {
2684         struct cake_sched_data *q = qdisc_priv(sch);
2685
2686         qdisc_watchdog_cancel(&q->watchdog);
2687         tcf_block_put(q->block);
2688         kvfree(q->tins);
2689 }
2690
2691 static int cake_init(struct Qdisc *sch, struct nlattr *opt,
2692                      struct netlink_ext_ack *extack)
2693 {
2694         struct cake_sched_data *q = qdisc_priv(sch);
2695         int i, j, err;
2696
2697         sch->limit = 10240;
2698         q->tin_mode = CAKE_DIFFSERV_DIFFSERV3;
2699         q->flow_mode  = CAKE_FLOW_TRIPLE;
2700
2701         q->rate_bps = 0; /* unlimited by default */
2702
2703         q->interval = 100000; /* 100ms default */
2704         q->target   =   5000; /* 5ms: codel RFC argues
2705                                * for 5 to 10% of interval
2706                                */
2707         q->rate_flags |= CAKE_FLAG_SPLIT_GSO;
2708         q->cur_tin = 0;
2709         q->cur_flow  = 0;
2710
2711         qdisc_watchdog_init(&q->watchdog, sch);
2712
2713         if (opt) {
2714                 err = cake_change(sch, opt, extack);
2715
2716                 if (err)
2717                         return err;
2718         }
2719
2720         err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
2721         if (err)
2722                 return err;
2723
2724         quantum_div[0] = ~0;
2725         for (i = 1; i <= CAKE_QUEUES; i++)
2726                 quantum_div[i] = 65535 / i;
2727
2728         q->tins = kvcalloc(CAKE_MAX_TINS, sizeof(struct cake_tin_data),
2729                            GFP_KERNEL);
2730         if (!q->tins)
2731                 return -ENOMEM;
2732
2733         for (i = 0; i < CAKE_MAX_TINS; i++) {
2734                 struct cake_tin_data *b = q->tins + i;
2735
2736                 INIT_LIST_HEAD(&b->new_flows);
2737                 INIT_LIST_HEAD(&b->old_flows);
2738                 INIT_LIST_HEAD(&b->decaying_flows);
2739                 b->sparse_flow_count = 0;
2740                 b->bulk_flow_count = 0;
2741                 b->decaying_flow_count = 0;
2742
2743                 for (j = 0; j < CAKE_QUEUES; j++) {
2744                         struct cake_flow *flow = b->flows + j;
2745                         u32 k = j * CAKE_MAX_TINS + i;
2746
2747                         INIT_LIST_HEAD(&flow->flowchain);
2748                         cobalt_vars_init(&flow->cvars);
2749
2750                         q->overflow_heap[k].t = i;
2751                         q->overflow_heap[k].b = j;
2752                         b->overflow_idx[j] = k;
2753                 }
2754         }
2755
2756         cake_reconfigure(sch);
2757         q->avg_peak_bandwidth = q->rate_bps;
2758         q->min_netlen = ~0;
2759         q->min_adjlen = ~0;
2760         return 0;
2761 }
2762
2763 static int cake_dump(struct Qdisc *sch, struct sk_buff *skb)
2764 {
2765         struct cake_sched_data *q = qdisc_priv(sch);
2766         struct nlattr *opts;
2767
2768         opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
2769         if (!opts)
2770                 goto nla_put_failure;
2771
2772         if (nla_put_u64_64bit(skb, TCA_CAKE_BASE_RATE64, q->rate_bps,
2773                               TCA_CAKE_PAD))
2774                 goto nla_put_failure;
2775
2776         if (nla_put_u32(skb, TCA_CAKE_FLOW_MODE,
2777                         q->flow_mode & CAKE_FLOW_MASK))
2778                 goto nla_put_failure;
2779
2780         if (nla_put_u32(skb, TCA_CAKE_RTT, q->interval))
2781                 goto nla_put_failure;
2782
2783         if (nla_put_u32(skb, TCA_CAKE_TARGET, q->target))
2784                 goto nla_put_failure;
2785
2786         if (nla_put_u32(skb, TCA_CAKE_MEMORY, q->buffer_config_limit))
2787                 goto nla_put_failure;
2788
2789         if (nla_put_u32(skb, TCA_CAKE_AUTORATE,
2790                         !!(q->rate_flags & CAKE_FLAG_AUTORATE_INGRESS)))
2791                 goto nla_put_failure;
2792
2793         if (nla_put_u32(skb, TCA_CAKE_INGRESS,
2794                         !!(q->rate_flags & CAKE_FLAG_INGRESS)))
2795                 goto nla_put_failure;
2796
2797         if (nla_put_u32(skb, TCA_CAKE_ACK_FILTER, q->ack_filter))
2798                 goto nla_put_failure;
2799
2800         if (nla_put_u32(skb, TCA_CAKE_NAT,
2801                         !!(q->flow_mode & CAKE_FLOW_NAT_FLAG)))
2802                 goto nla_put_failure;
2803
2804         if (nla_put_u32(skb, TCA_CAKE_DIFFSERV_MODE, q->tin_mode))
2805                 goto nla_put_failure;
2806
2807         if (nla_put_u32(skb, TCA_CAKE_WASH,
2808                         !!(q->rate_flags & CAKE_FLAG_WASH)))
2809                 goto nla_put_failure;
2810
2811         if (nla_put_u32(skb, TCA_CAKE_OVERHEAD, q->rate_overhead))
2812                 goto nla_put_failure;
2813
2814         if (!(q->rate_flags & CAKE_FLAG_OVERHEAD))
2815                 if (nla_put_u32(skb, TCA_CAKE_RAW, 0))
2816                         goto nla_put_failure;
2817
2818         if (nla_put_u32(skb, TCA_CAKE_ATM, q->atm_mode))
2819                 goto nla_put_failure;
2820
2821         if (nla_put_u32(skb, TCA_CAKE_MPU, q->rate_mpu))
2822                 goto nla_put_failure;
2823
2824         if (nla_put_u32(skb, TCA_CAKE_SPLIT_GSO,
2825                         !!(q->rate_flags & CAKE_FLAG_SPLIT_GSO)))
2826                 goto nla_put_failure;
2827
2828         if (nla_put_u32(skb, TCA_CAKE_FWMARK, q->fwmark_mask))
2829                 goto nla_put_failure;
2830
2831         return nla_nest_end(skb, opts);
2832
2833 nla_put_failure:
2834         return -1;
2835 }
2836
2837 static int cake_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
2838 {
2839         struct nlattr *stats = nla_nest_start_noflag(d->skb, TCA_STATS_APP);
2840         struct cake_sched_data *q = qdisc_priv(sch);
2841         struct nlattr *tstats, *ts;
2842         int i;
2843
2844         if (!stats)
2845                 return -1;
2846
2847 #define PUT_STAT_U32(attr, data) do {                                  \
2848                 if (nla_put_u32(d->skb, TCA_CAKE_STATS_ ## attr, data)) \
2849                         goto nla_put_failure;                          \
2850         } while (0)
2851 #define PUT_STAT_U64(attr, data) do {                                  \
2852                 if (nla_put_u64_64bit(d->skb, TCA_CAKE_STATS_ ## attr, \
2853                                         data, TCA_CAKE_STATS_PAD)) \
2854                         goto nla_put_failure;                          \
2855         } while (0)
2856
2857         PUT_STAT_U64(CAPACITY_ESTIMATE64, q->avg_peak_bandwidth);
2858         PUT_STAT_U32(MEMORY_LIMIT, q->buffer_limit);
2859         PUT_STAT_U32(MEMORY_USED, q->buffer_max_used);
2860         PUT_STAT_U32(AVG_NETOFF, ((q->avg_netoff + 0x8000) >> 16));
2861         PUT_STAT_U32(MAX_NETLEN, q->max_netlen);
2862         PUT_STAT_U32(MAX_ADJLEN, q->max_adjlen);
2863         PUT_STAT_U32(MIN_NETLEN, q->min_netlen);
2864         PUT_STAT_U32(MIN_ADJLEN, q->min_adjlen);
2865
2866 #undef PUT_STAT_U32
2867 #undef PUT_STAT_U64
2868
2869         tstats = nla_nest_start_noflag(d->skb, TCA_CAKE_STATS_TIN_STATS);
2870         if (!tstats)
2871                 goto nla_put_failure;
2872
2873 #define PUT_TSTAT_U32(attr, data) do {                                  \
2874                 if (nla_put_u32(d->skb, TCA_CAKE_TIN_STATS_ ## attr, data)) \
2875                         goto nla_put_failure;                           \
2876         } while (0)
2877 #define PUT_TSTAT_U64(attr, data) do {                                  \
2878                 if (nla_put_u64_64bit(d->skb, TCA_CAKE_TIN_STATS_ ## attr, \
2879                                         data, TCA_CAKE_TIN_STATS_PAD))  \
2880                         goto nla_put_failure;                           \
2881         } while (0)
2882
2883         for (i = 0; i < q->tin_cnt; i++) {
2884                 struct cake_tin_data *b = &q->tins[q->tin_order[i]];
2885
2886                 ts = nla_nest_start_noflag(d->skb, i + 1);
2887                 if (!ts)
2888                         goto nla_put_failure;
2889
2890                 PUT_TSTAT_U64(THRESHOLD_RATE64, b->tin_rate_bps);
2891                 PUT_TSTAT_U64(SENT_BYTES64, b->bytes);
2892                 PUT_TSTAT_U32(BACKLOG_BYTES, b->tin_backlog);
2893
2894                 PUT_TSTAT_U32(TARGET_US,
2895                               ktime_to_us(ns_to_ktime(b->cparams.target)));
2896                 PUT_TSTAT_U32(INTERVAL_US,
2897                               ktime_to_us(ns_to_ktime(b->cparams.interval)));
2898
2899                 PUT_TSTAT_U32(SENT_PACKETS, b->packets);
2900                 PUT_TSTAT_U32(DROPPED_PACKETS, b->tin_dropped);
2901                 PUT_TSTAT_U32(ECN_MARKED_PACKETS, b->tin_ecn_mark);
2902                 PUT_TSTAT_U32(ACKS_DROPPED_PACKETS, b->ack_drops);
2903
2904                 PUT_TSTAT_U32(PEAK_DELAY_US,
2905                               ktime_to_us(ns_to_ktime(b->peak_delay)));
2906                 PUT_TSTAT_U32(AVG_DELAY_US,
2907                               ktime_to_us(ns_to_ktime(b->avge_delay)));
2908                 PUT_TSTAT_U32(BASE_DELAY_US,
2909                               ktime_to_us(ns_to_ktime(b->base_delay)));
2910
2911                 PUT_TSTAT_U32(WAY_INDIRECT_HITS, b->way_hits);
2912                 PUT_TSTAT_U32(WAY_MISSES, b->way_misses);
2913                 PUT_TSTAT_U32(WAY_COLLISIONS, b->way_collisions);
2914
2915                 PUT_TSTAT_U32(SPARSE_FLOWS, b->sparse_flow_count +
2916                                             b->decaying_flow_count);
2917                 PUT_TSTAT_U32(BULK_FLOWS, b->bulk_flow_count);
2918                 PUT_TSTAT_U32(UNRESPONSIVE_FLOWS, b->unresponsive_flow_count);
2919                 PUT_TSTAT_U32(MAX_SKBLEN, b->max_skblen);
2920
2921                 PUT_TSTAT_U32(FLOW_QUANTUM, b->flow_quantum);
2922                 nla_nest_end(d->skb, ts);
2923         }
2924
2925 #undef PUT_TSTAT_U32
2926 #undef PUT_TSTAT_U64
2927
2928         nla_nest_end(d->skb, tstats);
2929         return nla_nest_end(d->skb, stats);
2930
2931 nla_put_failure:
2932         nla_nest_cancel(d->skb, stats);
2933         return -1;
2934 }
2935
2936 static struct Qdisc *cake_leaf(struct Qdisc *sch, unsigned long arg)
2937 {
2938         return NULL;
2939 }
2940
2941 static unsigned long cake_find(struct Qdisc *sch, u32 classid)
2942 {
2943         return 0;
2944 }
2945
2946 static unsigned long cake_bind(struct Qdisc *sch, unsigned long parent,
2947                                u32 classid)
2948 {
2949         return 0;
2950 }
2951
2952 static void cake_unbind(struct Qdisc *q, unsigned long cl)
2953 {
2954 }
2955
2956 static struct tcf_block *cake_tcf_block(struct Qdisc *sch, unsigned long cl,
2957                                         struct netlink_ext_ack *extack)
2958 {
2959         struct cake_sched_data *q = qdisc_priv(sch);
2960
2961         if (cl)
2962                 return NULL;
2963         return q->block;
2964 }
2965
2966 static int cake_dump_class(struct Qdisc *sch, unsigned long cl,
2967                            struct sk_buff *skb, struct tcmsg *tcm)
2968 {
2969         tcm->tcm_handle |= TC_H_MIN(cl);
2970         return 0;
2971 }
2972
2973 static int cake_dump_class_stats(struct Qdisc *sch, unsigned long cl,
2974                                  struct gnet_dump *d)
2975 {
2976         struct cake_sched_data *q = qdisc_priv(sch);
2977         const struct cake_flow *flow = NULL;
2978         struct gnet_stats_queue qs = { 0 };
2979         struct nlattr *stats;
2980         u32 idx = cl - 1;
2981
2982         if (idx < CAKE_QUEUES * q->tin_cnt) {
2983                 const struct cake_tin_data *b = \
2984                         &q->tins[q->tin_order[idx / CAKE_QUEUES]];
2985                 const struct sk_buff *skb;
2986
2987                 flow = &b->flows[idx % CAKE_QUEUES];
2988
2989                 if (flow->head) {
2990                         sch_tree_lock(sch);
2991                         skb = flow->head;
2992                         while (skb) {
2993                                 qs.qlen++;
2994                                 skb = skb->next;
2995                         }
2996                         sch_tree_unlock(sch);
2997                 }
2998                 qs.backlog = b->backlogs[idx % CAKE_QUEUES];
2999                 qs.drops = flow->dropped;
3000         }
3001         if (gnet_stats_copy_queue(d, NULL, &qs, qs.qlen) < 0)
3002                 return -1;
3003         if (flow) {
3004                 ktime_t now = ktime_get();
3005
3006                 stats = nla_nest_start_noflag(d->skb, TCA_STATS_APP);
3007                 if (!stats)
3008                         return -1;
3009
3010 #define PUT_STAT_U32(attr, data) do {                                  \
3011                 if (nla_put_u32(d->skb, TCA_CAKE_STATS_ ## attr, data)) \
3012                         goto nla_put_failure;                          \
3013         } while (0)
3014 #define PUT_STAT_S32(attr, data) do {                                  \
3015                 if (nla_put_s32(d->skb, TCA_CAKE_STATS_ ## attr, data)) \
3016                         goto nla_put_failure;                          \
3017         } while (0)
3018
3019                 PUT_STAT_S32(DEFICIT, flow->deficit);
3020                 PUT_STAT_U32(DROPPING, flow->cvars.dropping);
3021                 PUT_STAT_U32(COBALT_COUNT, flow->cvars.count);
3022                 PUT_STAT_U32(P_DROP, flow->cvars.p_drop);
3023                 if (flow->cvars.p_drop) {
3024                         PUT_STAT_S32(BLUE_TIMER_US,
3025                                      ktime_to_us(
3026                                              ktime_sub(now,
3027                                                        flow->cvars.blue_timer)));
3028                 }
3029                 if (flow->cvars.dropping) {
3030                         PUT_STAT_S32(DROP_NEXT_US,
3031                                      ktime_to_us(
3032                                              ktime_sub(now,
3033                                                        flow->cvars.drop_next)));
3034                 }
3035
3036                 if (nla_nest_end(d->skb, stats) < 0)
3037                         return -1;
3038         }
3039
3040         return 0;
3041
3042 nla_put_failure:
3043         nla_nest_cancel(d->skb, stats);
3044         return -1;
3045 }
3046
3047 static void cake_walk(struct Qdisc *sch, struct qdisc_walker *arg)
3048 {
3049         struct cake_sched_data *q = qdisc_priv(sch);
3050         unsigned int i, j;
3051
3052         if (arg->stop)
3053                 return;
3054
3055         for (i = 0; i < q->tin_cnt; i++) {
3056                 struct cake_tin_data *b = &q->tins[q->tin_order[i]];
3057
3058                 for (j = 0; j < CAKE_QUEUES; j++) {
3059                         if (list_empty(&b->flows[j].flowchain) ||
3060                             arg->count < arg->skip) {
3061                                 arg->count++;
3062                                 continue;
3063                         }
3064                         if (arg->fn(sch, i * CAKE_QUEUES + j + 1, arg) < 0) {
3065                                 arg->stop = 1;
3066                                 break;
3067                         }
3068                         arg->count++;
3069                 }
3070         }
3071 }
3072
3073 static const struct Qdisc_class_ops cake_class_ops = {
3074         .leaf           =       cake_leaf,
3075         .find           =       cake_find,
3076         .tcf_block      =       cake_tcf_block,
3077         .bind_tcf       =       cake_bind,
3078         .unbind_tcf     =       cake_unbind,
3079         .dump           =       cake_dump_class,
3080         .dump_stats     =       cake_dump_class_stats,
3081         .walk           =       cake_walk,
3082 };
3083
3084 static struct Qdisc_ops cake_qdisc_ops __read_mostly = {
3085         .cl_ops         =       &cake_class_ops,
3086         .id             =       "cake",
3087         .priv_size      =       sizeof(struct cake_sched_data),
3088         .enqueue        =       cake_enqueue,
3089         .dequeue        =       cake_dequeue,
3090         .peek           =       qdisc_peek_dequeued,
3091         .init           =       cake_init,
3092         .reset          =       cake_reset,
3093         .destroy        =       cake_destroy,
3094         .change         =       cake_change,
3095         .dump           =       cake_dump,
3096         .dump_stats     =       cake_dump_stats,
3097         .owner          =       THIS_MODULE,
3098 };
3099
3100 static int __init cake_module_init(void)
3101 {
3102         return register_qdisc(&cake_qdisc_ops);
3103 }
3104
3105 static void __exit cake_module_exit(void)
3106 {
3107         unregister_qdisc(&cake_qdisc_ops);
3108 }
3109
3110 module_init(cake_module_init)
3111 module_exit(cake_module_exit)
3112 MODULE_AUTHOR("Jonathan Morton");
3113 MODULE_LICENSE("Dual BSD/GPL");
3114 MODULE_DESCRIPTION("The CAKE shaper.");