net/sched/sch_netem.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * net/sched/sch_netem.c        Network emulator
   4  *
   5  *              Many of the algorithms and ideas for this came from
   6  *              NIST Net which is not copyrighted.
   7  *
   8  * Authors:     Stephen Hemminger <shemminger@osdl.org>
   9  *              Catalin(ux aka Dino) BOIE <catab at umbrella dot ro>
  10  */
  11
  12 #include <linux/mm.h>
  13 #include <linux/module.h>
  14 #include <linux/slab.h>
  15 #include <linux/types.h>
  16 #include <linux/kernel.h>
  17 #include <linux/errno.h>
  18 #include <linux/skbuff.h>
  19 #include <linux/vmalloc.h>
  20 #include <linux/rtnetlink.h>
  21 #include <linux/reciprocal_div.h>
  22 #include <linux/rbtree.h>
  23
  24 #include <net/netlink.h>
  25 #include <net/pkt_sched.h>
  26 #include <net/inet_ecn.h>
  27
  28 #define VERSION "1.3"
  29
  30 /*      Network Emulation Queuing algorithm.
  31         ====================================
  32
  33         Sources: [1] Mark Carson, Darrin Santay, "NIST Net - A Linux-based
  34                  Network Emulation Tool
  35                  [2] Luigi Rizzo, DummyNet for FreeBSD
  36
  37          ----------------------------------------------------------------
  38
  39          This started out as a simple way to delay outgoing packets to
  40          test TCP but has grown to include most of the functionality
  41          of a full blown network emulator like NISTnet. It can delay
  42          packets and add random jitter (and correlation). The random
  43          distribution can be loaded from a table as well to provide
  44          normal, Pareto, or experimental curves. Packet loss,
  45          duplication, and reordering can also be emulated.
  46
  47          This qdisc does not do classification that can be handled in
  48          layering other disciplines.  It does not need to do bandwidth
  49          control either since that can be handled by using token
  50          bucket or other rate control.
  51
  52      Correlated Loss Generator models
  53
  54         Added generation of correlated loss according to the
  55         "Gilbert-Elliot" model, a 4-state markov model.
  56
  57         References:
  58         [1] NetemCLG Home http://netgroup.uniroma2.it/NetemCLG
  59         [2] S. Salsano, F. Ludovici, A. Ordine, "Definition of a general
  60         and intuitive loss model for packet networks and its implementation
  61         in the Netem module in the Linux kernel", available in [1]
  62
  63         Authors: Stefano Salsano <stefano.salsano at uniroma2.it
  64                  Fabio Ludovici <fabio.ludovici at yahoo.it>
  65 */
  66
  67 struct disttable {
  68         u32  size;
  69         s16 table[0];
  70 };
  71
  72 struct netem_sched_data {
  73         /* internal t(ime)fifo qdisc uses t_root and sch->limit */
  74         struct rb_root t_root;
  75
  76         /* a linear queue; reduces rbtree rebalancing when jitter is low */
  77         struct sk_buff  *t_head;
  78         struct sk_buff  *t_tail;
  79
  80         /* optional qdisc for classful handling (NULL at netem init) */
  81         struct Qdisc    *qdisc;
  82
  83         struct qdisc_watchdog watchdog;
  84
  85         s64 latency;
  86         s64 jitter;
  87
  88         u32 loss;
  89         u32 ecn;
  90         u32 limit;
  91         u32 counter;
  92         u32 gap;
  93         u32 duplicate;
  94         u32 reorder;
  95         u32 corrupt;
  96         u64 rate;
  97         s32 packet_overhead;
  98         u32 cell_size;
  99         struct reciprocal_value cell_size_reciprocal;
 100         s32 cell_overhead;
 101
 102         struct crndstate {
 103                 u32 last;
 104                 u32 rho;
 105         } delay_cor, loss_cor, dup_cor, reorder_cor, corrupt_cor;
 106
 107         struct disttable *delay_dist;
 108
 109         enum  {
 110                 CLG_RANDOM,
 111                 CLG_4_STATES,
 112                 CLG_GILB_ELL,
 113         } loss_model;
 114
 115         enum {
 116                 TX_IN_GAP_PERIOD = 1,
 117                 TX_IN_BURST_PERIOD,
 118                 LOST_IN_GAP_PERIOD,
 119                 LOST_IN_BURST_PERIOD,
 120         } _4_state_model;
 121
 122         enum {
 123                 GOOD_STATE = 1,
 124                 BAD_STATE,
 125         } GE_state_model;
 126
 127         /* Correlated Loss Generation models */
 128         struct clgstate {
 129                 /* state of the Markov chain */
 130                 u8 state;
 131
 132                 /* 4-states and Gilbert-Elliot models */
 133                 u32 a1; /* p13 for 4-states or p for GE */
 134                 u32 a2; /* p31 for 4-states or r for GE */
 135                 u32 a3; /* p32 for 4-states or h for GE */
 136                 u32 a4; /* p14 for 4-states or 1-k for GE */
 137                 u32 a5; /* p23 used only in 4-states */
 138         } clg;
 139
 140         struct tc_netem_slot slot_config;
 141         struct slotstate {
 142                 u64 slot_next;
 143                 s32 packets_left;
 144                 s32 bytes_left;
 145         } slot;
 146
 147         struct disttable *slot_dist;
 148 };
 149
 150 /* Time stamp put into socket buffer control block
 151  * Only valid when skbs are in our internal t(ime)fifo queue.
 152  *
 153  * As skb->rbnode uses same storage than skb->next, skb->prev and skb->tstamp,
 154  * and skb->next & skb->prev are scratch space for a qdisc,
 155  * we save skb->tstamp value in skb->cb[] before destroying it.
 156  */
 157 struct netem_skb_cb {
 158         u64             time_to_send;
 159 };
 160
 161 static inline struct netem_skb_cb *netem_skb_cb(struct sk_buff *skb)
 162 {
 163         /* we assume we can use skb next/prev/tstamp as storage for rb_node */
 164         qdisc_cb_private_validate(skb, sizeof(struct netem_skb_cb));
 165         return (struct netem_skb_cb *)qdisc_skb_cb(skb)->data;
 166 }
 167
 168 /* init_crandom - initialize correlated random number generator
 169  * Use entropy source for initial seed.
 170  */
 171 static void init_crandom(struct crndstate *state, unsigned long rho)
 172 {
 173         state->rho = rho;
 174         state->last = prandom_u32();
 175 }
 176
 177 /* get_crandom - correlated random number generator
 178  * Next number depends on last value.
 179  * rho is scaled to avoid floating point.
 180  */
 181 static u32 get_crandom(struct crndstate *state)
 182 {
 183         u64 value, rho;
 184         unsigned long answer;
 185
 186         if (!state || state->rho == 0)  /* no correlation */
 187                 return prandom_u32();
 188
 189         value = prandom_u32();
 190         rho = (u64)state->rho + 1;
 191         answer = (value * ((1ull<<32) - rho) + state->last * rho) >> 32;
 192         state->last = answer;
 193         return answer;
 194 }
 195
 196 /* loss_4state - 4-state model loss generator
 197  * Generates losses according to the 4-state Markov chain adopted in
 198  * the GI (General and Intuitive) loss model.
 199  */
 200 static bool loss_4state(struct netem_sched_data *q)
 201 {
 202         struct clgstate *clg = &q->clg;
 203         u32 rnd = prandom_u32();
 204
 205         /*
 206          * Makes a comparison between rnd and the transition
 207          * probabilities outgoing from the current state, then decides the
 208          * next state and if the next packet has to be transmitted or lost.
 209          * The four states correspond to:
 210          *   TX_IN_GAP_PERIOD => successfully transmitted packets within a gap period
 211          *   LOST_IN_BURST_PERIOD => isolated losses within a gap period
 212          *   LOST_IN_GAP_PERIOD => lost packets within a burst period
 213          *   TX_IN_GAP_PERIOD => successfully transmitted packets within a burst period
 214          */
 215         switch (clg->state) {
 216         case TX_IN_GAP_PERIOD:
 217                 if (rnd < clg->a4) {
 218                         clg->state = LOST_IN_BURST_PERIOD;
 219                         return true;
 220                 } else if (clg->a4 < rnd && rnd < clg->a1 + clg->a4) {
 221                         clg->state = LOST_IN_GAP_PERIOD;
 222                         return true;
 223                 } else if (clg->a1 + clg->a4 < rnd) {
 224                         clg->state = TX_IN_GAP_PERIOD;
 225                 }
 226
 227                 break;
 228         case TX_IN_BURST_PERIOD:
 229                 if (rnd < clg->a5) {
 230                         clg->state = LOST_IN_GAP_PERIOD;
 231                         return true;
 232                 } else {
 233                         clg->state = TX_IN_BURST_PERIOD;
 234                 }
 235
 236                 break;
 237         case LOST_IN_GAP_PERIOD:
 238                 if (rnd < clg->a3)
 239                         clg->state = TX_IN_BURST_PERIOD;
 240                 else if (clg->a3 < rnd && rnd < clg->a2 + clg->a3) {
 241                         clg->state = TX_IN_GAP_PERIOD;
 242                 } else if (clg->a2 + clg->a3 < rnd) {
 243                         clg->state = LOST_IN_GAP_PERIOD;
 244                         return true;
 245                 }
 246                 break;
 247         case LOST_IN_BURST_PERIOD:
 248                 clg->state = TX_IN_GAP_PERIOD;
 249                 break;
 250         }
 251
 252         return false;
 253 }
 254
 255 /* loss_gilb_ell - Gilbert-Elliot model loss generator
 256  * Generates losses according to the Gilbert-Elliot loss model or
 257  * its special cases  (Gilbert or Simple Gilbert)
 258  *
 259  * Makes a comparison between random number and the transition
 260  * probabilities outgoing from the current state, then decides the
 261  * next state. A second random number is extracted and the comparison
 262  * with the loss probability of the current state decides if the next
 263  * packet will be transmitted or lost.
 264  */
 265 static bool loss_gilb_ell(struct netem_sched_data *q)
 266 {
 267         struct clgstate *clg = &q->clg;
 268
 269         switch (clg->state) {
 270         case GOOD_STATE:
 271                 if (prandom_u32() < clg->a1)
 272                         clg->state = BAD_STATE;
 273                 if (prandom_u32() < clg->a4)
 274                         return true;
 275                 break;
 276         case BAD_STATE:
 277                 if (prandom_u32() < clg->a2)
 278                         clg->state = GOOD_STATE;
 279                 if (prandom_u32() > clg->a3)
 280                         return true;
 281         }
 282
 283         return false;
 284 }
 285
 286 static bool loss_event(struct netem_sched_data *q)
 287 {
 288         switch (q->loss_model) {
 289         case CLG_RANDOM:
 290                 /* Random packet drop 0 => none, ~0 => all */
 291                 return q->loss && q->loss >= get_crandom(&q->loss_cor);
 292
 293         case CLG_4_STATES:
 294                 /* 4state loss model algorithm (used also for GI model)
 295                 * Extracts a value from the markov 4 state loss generator,
 296                 * if it is 1 drops a packet and if needed writes the event in
 297                 * the kernel logs
 298                 */
 299                 return loss_4state(q);
 300
 301         case CLG_GILB_ELL:
 302                 /* Gilbert-Elliot loss model algorithm
 303                 * Extracts a value from the Gilbert-Elliot loss generator,
 304                 * if it is 1 drops a packet and if needed writes the event in
 305                 * the kernel logs
 306                 */
 307                 return loss_gilb_ell(q);
 308         }
 309
 310         return false;   /* not reached */
 311 }
 312
 313
 314 /* tabledist - return a pseudo-randomly distributed value with mean mu and
 315  * std deviation sigma.  Uses table lookup to approximate the desired
 316  * distribution, and a uniformly-distributed pseudo-random source.
 317  */
 318 static s64 tabledist(s64 mu, s32 sigma,
 319                      struct crndstate *state,
 320                      const struct disttable *dist)
 321 {
 322         s64 x;
 323         long t;
 324         u32 rnd;
 325
 326         if (sigma == 0)
 327                 return mu;
 328
 329         rnd = get_crandom(state);
 330
 331         /* default uniform distribution */
 332         if (dist == NULL)
 333                 return ((rnd % (2 * sigma)) + mu) - sigma;
 334
 335         t = dist->table[rnd % dist->size];
 336         x = (sigma % NETEM_DIST_SCALE) * t;
 337         if (x >= 0)
 338                 x += NETEM_DIST_SCALE/2;
 339         else
 340                 x -= NETEM_DIST_SCALE/2;
 341
 342         return  x / NETEM_DIST_SCALE + (sigma / NETEM_DIST_SCALE) * t + mu;
 343 }
 344
 345 static u64 packet_time_ns(u64 len, const struct netem_sched_data *q)
 346 {
 347         len += q->packet_overhead;
 348
 349         if (q->cell_size) {
 350                 u32 cells = reciprocal_divide(len, q->cell_size_reciprocal);
 351
 352                 if (len > cells * q->cell_size) /* extra cell needed for remainder */
 353                         cells++;
 354                 len = cells * (q->cell_size + q->cell_overhead);
 355         }
 356
 357         return div64_u64(len * NSEC_PER_SEC, q->rate);
 358 }
 359
 360 static void tfifo_reset(struct Qdisc *sch)
 361 {
 362         struct netem_sched_data *q = qdisc_priv(sch);
 363         struct rb_node *p = rb_first(&q->t_root);
 364
 365         while (p) {
 366                 struct sk_buff *skb = rb_to_skb(p);
 367
 368                 p = rb_next(p);
 369                 rb_erase(&skb->rbnode, &q->t_root);
 370                 rtnl_kfree_skbs(skb, skb);
 371         }
 372
 373         rtnl_kfree_skbs(q->t_head, q->t_tail);
 374         q->t_head = NULL;
 375         q->t_tail = NULL;
 376 }
 377
 378 static void tfifo_enqueue(struct sk_buff *nskb, struct Qdisc *sch)
 379 {
 380         struct netem_sched_data *q = qdisc_priv(sch);
 381         u64 tnext = netem_skb_cb(nskb)->time_to_send;
 382
 383         if (!q->t_tail || tnext >= netem_skb_cb(q->t_tail)->time_to_send) {
 384                 if (q->t_tail)
 385                         q->t_tail->next = nskb;
 386                 else
 387                         q->t_head = nskb;
 388                 q->t_tail = nskb;
 389         } else {
 390                 struct rb_node **p = &q->t_root.rb_node, *parent = NULL;
 391
 392                 while (*p) {
 393                         struct sk_buff *skb;
 394
 395                         parent = *p;
 396                         skb = rb_to_skb(parent);
 397                         if (tnext >= netem_skb_cb(skb)->time_to_send)
 398                                 p = &parent->rb_right;
 399                         else
 400                                 p = &parent->rb_left;
 401                 }
 402                 rb_link_node(&nskb->rbnode, parent, p);
 403                 rb_insert_color(&nskb->rbnode, &q->t_root);
 404         }
 405         sch->q.qlen++;
 406 }
 407
 408 /* netem can't properly corrupt a megapacket (like we get from GSO), so instead
 409  * when we statistically choose to corrupt one, we instead segment it, returning
 410  * the first packet to be corrupted, and re-enqueue the remaining frames
 411  */
 412 static struct sk_buff *netem_segment(struct sk_buff *skb, struct Qdisc *sch,
 413                                      struct sk_buff **to_free)
 414 {
 415         struct sk_buff *segs;
 416         netdev_features_t features = netif_skb_features(skb);
 417
 418         segs = skb_gso_segment(skb, features & ~NETIF_F_GSO_MASK);
 419
 420         if (IS_ERR_OR_NULL(segs)) {
 421                 qdisc_drop(skb, sch, to_free);
 422                 return NULL;
 423         }
 424         consume_skb(skb);
 425         return segs;
 426 }
 427
 428 /*
 429  * Insert one skb into qdisc.
 430  * Note: parent depends on return value to account for queue length.
 431  *      NET_XMIT_DROP: queue length didn't change.
 432  *      NET_XMIT_SUCCESS: one skb was queued.
 433  */
 434 static int netem_enqueue(struct sk_buff *skb, struct Qdisc *sch,
 435                          struct sk_buff **to_free)
 436 {
 437         struct netem_sched_data *q = qdisc_priv(sch);
 438         /* We don't fill cb now as skb_unshare() may invalidate it */
 439         struct netem_skb_cb *cb;
 440         struct sk_buff *skb2;
 441         struct sk_buff *segs = NULL;
 442         unsigned int prev_len = qdisc_pkt_len(skb);
 443         int count = 1;
 444         int rc = NET_XMIT_SUCCESS;
 445         int rc_drop = NET_XMIT_DROP;
 446
 447         /* Do not fool qdisc_drop_all() */
 448         skb->prev = NULL;
 449
 450         /* Random duplication */
 451         if (q->duplicate && q->duplicate >= get_crandom(&q->dup_cor))
 452                 ++count;
 453
 454         /* Drop packet? */
 455         if (loss_event(q)) {
 456                 if (q->ecn && INET_ECN_set_ce(skb))
 457                         qdisc_qstats_drop(sch); /* mark packet */
 458                 else
 459                         --count;
 460         }
 461         if (count == 0) {
 462                 qdisc_qstats_drop(sch);
 463                 __qdisc_drop(skb, to_free);
 464                 return NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
 465         }
 466
 467         /* If a delay is expected, orphan the skb. (orphaning usually takes
 468          * place at TX completion time, so _before_ the link transit delay)
 469          */
 470         if (q->latency || q->jitter || q->rate)
 471                 skb_orphan_partial(skb);
 472
 473         /*
 474          * If we need to duplicate packet, then re-insert at top of the
 475          * qdisc tree, since parent queuer expects that only one
 476          * skb will be queued.
 477          */
 478         if (count > 1 && (skb2 = skb_clone(skb, GFP_ATOMIC)) != NULL) {
 479                 struct Qdisc *rootq = qdisc_root(sch);
 480                 u32 dupsave = q->duplicate; /* prevent duplicating a dup... */
 481
 482                 q->duplicate = 0;
 483                 rootq->enqueue(skb2, rootq, to_free);
 484                 q->duplicate = dupsave;
 485                 rc_drop = NET_XMIT_SUCCESS;
 486         }
 487
 488         /*
 489          * Randomized packet corruption.
 490          * Make copy if needed since we are modifying
 491          * If packet is going to be hardware checksummed, then
 492          * do it now in software before we mangle it.
 493          */
 494         if (q->corrupt && q->corrupt >= get_crandom(&q->corrupt_cor)) {
 495                 if (skb_is_gso(skb)) {
 496                         skb = netem_segment(skb, sch, to_free);
 497                         if (!skb)
 498                                 return rc_drop;
 499                         segs = skb->next;
 500                         skb_mark_not_on_list(skb);
 501                         qdisc_skb_cb(skb)->pkt_len = skb->len;
 502                 }
 503
 504                 skb = skb_unshare(skb, GFP_ATOMIC);
 505                 if (unlikely(!skb)) {
 506                         qdisc_qstats_drop(sch);
 507                         goto finish_segs;
 508                 }
 509                 if (skb->ip_summed == CHECKSUM_PARTIAL &&
 510                     skb_checksum_help(skb)) {
 511                         qdisc_drop(skb, sch, to_free);
 512                         goto finish_segs;
 513                 }
 514
 515                 skb->data[prandom_u32() % skb_headlen(skb)] ^=
 516                         1<<(prandom_u32() % 8);
 517         }
 518
 519         if (unlikely(sch->q.qlen >= sch->limit)) {
 520                 /* re-link segs, so that qdisc_drop_all() frees them all */
 521                 skb->next = segs;
 522                 qdisc_drop_all(skb, sch, to_free);
 523                 return rc_drop;
 524         }
 525
 526         qdisc_qstats_backlog_inc(sch, skb);
 527
 528         cb = netem_skb_cb(skb);
 529         if (q->gap == 0 ||              /* not doing reordering */
 530             q->counter < q->gap - 1 ||  /* inside last reordering gap */
 531             q->reorder < get_crandom(&q->reorder_cor)) {
 532                 u64 now;
 533                 s64 delay;
 534
 535                 delay = tabledist(q->latency, q->jitter,
 536                                   &q->delay_cor, q->delay_dist);
 537
 538                 now = ktime_get_ns();
 539
 540                 if (q->rate) {
 541                         struct netem_skb_cb *last = NULL;
 542
 543                         if (sch->q.tail)
 544                                 last = netem_skb_cb(sch->q.tail);
 545                         if (q->t_root.rb_node) {
 546                                 struct sk_buff *t_skb;
 547                                 struct netem_skb_cb *t_last;
 548
 549                                 t_skb = skb_rb_last(&q->t_root);
 550                                 t_last = netem_skb_cb(t_skb);
 551                                 if (!last ||
 552                                     t_last->time_to_send > last->time_to_send)
 553                                         last = t_last;
 554                         }
 555                         if (q->t_tail) {
 556                                 struct netem_skb_cb *t_last =
 557                                         netem_skb_cb(q->t_tail);
 558
 559                                 if (!last ||
 560                                     t_last->time_to_send > last->time_to_send)
 561                                         last = t_last;
 562                         }
 563
 564                         if (last) {
 565                                 /*
 566                                  * Last packet in queue is reference point (now),
 567                                  * calculate this time bonus and subtract
 568                                  * from delay.
 569                                  */
 570                                 delay -= last->time_to_send - now;
 571                                 delay = max_t(s64, 0, delay);
 572                                 now = last->time_to_send;
 573                         }
 574
 575                         delay += packet_time_ns(qdisc_pkt_len(skb), q);
 576                 }
 577
 578                 cb->time_to_send = now + delay;
 579                 ++q->counter;
 580                 tfifo_enqueue(skb, sch);
 581         } else {
 582                 /*
 583                  * Do re-ordering by putting one out of N packets at the front
 584                  * of the queue.
 585                  */
 586                 cb->time_to_send = ktime_get_ns();
 587                 q->counter = 0;
 588
 589                 __qdisc_enqueue_head(skb, &sch->q);
 590                 sch->qstats.requeues++;
 591         }
 592
 593 finish_segs:
 594         if (segs) {
 595                 unsigned int len, last_len;
 596                 int nb = 0;
 597
 598                 len = skb->len;
 599
 600                 while (segs) {
 601                         skb2 = segs->next;
 602                         skb_mark_not_on_list(segs);
 603                         qdisc_skb_cb(segs)->pkt_len = segs->len;
 604                         last_len = segs->len;
 605                         rc = qdisc_enqueue(segs, sch, to_free);
 606                         if (rc != NET_XMIT_SUCCESS) {
 607                                 if (net_xmit_drop_count(rc))
 608                                         qdisc_qstats_drop(sch);
 609                         } else {
 610                                 nb++;
 611                                 len += last_len;
 612                         }
 613                         segs = skb2;
 614                 }
 615                 qdisc_tree_reduce_backlog(sch, -nb, prev_len - len);
 616         }
 617         return NET_XMIT_SUCCESS;
 618 }
 619
 620 /* Delay the next round with a new future slot with a
 621  * correct number of bytes and packets.
 622  */
 623
 624 static void get_slot_next(struct netem_sched_data *q, u64 now)
 625 {
 626         s64 next_delay;
 627
 628         if (!q->slot_dist)
 629                 next_delay = q->slot_config.min_delay +
 630                                 (prandom_u32() *
 631                                  (q->slot_config.max_delay -
 632                                   q->slot_config.min_delay) >> 32);
 633         else
 634                 next_delay = tabledist(q->slot_config.dist_delay,
 635                                        (s32)(q->slot_config.dist_jitter),
 636                                        NULL, q->slot_dist);
 637
 638         q->slot.slot_next = now + next_delay;
 639         q->slot.packets_left = q->slot_config.max_packets;
 640         q->slot.bytes_left = q->slot_config.max_bytes;
 641 }
 642
 643 static struct sk_buff *netem_peek(struct netem_sched_data *q)
 644 {
 645         struct sk_buff *skb = skb_rb_first(&q->t_root);
 646         u64 t1, t2;
 647
 648         if (!skb)
 649                 return q->t_head;
 650         if (!q->t_head)
 651                 return skb;
 652
 653         t1 = netem_skb_cb(skb)->time_to_send;
 654         t2 = netem_skb_cb(q->t_head)->time_to_send;
 655         if (t1 < t2)
 656                 return skb;
 657         return q->t_head;
 658 }
 659
 660 static void netem_erase_head(struct netem_sched_data *q, struct sk_buff *skb)
 661 {
 662         if (skb == q->t_head) {
 663                 q->t_head = skb->next;
 664                 if (!q->t_head)
 665                         q->t_tail = NULL;
 666         } else {
 667                 rb_erase(&skb->rbnode, &q->t_root);
 668         }
 669 }
 670
 671 static struct sk_buff *netem_dequeue(struct Qdisc *sch)
 672 {
 673         struct netem_sched_data *q = qdisc_priv(sch);
 674         struct sk_buff *skb;
 675
 676 tfifo_dequeue:
 677         skb = __qdisc_dequeue_head(&sch->q);
 678         if (skb) {
 679                 qdisc_qstats_backlog_dec(sch, skb);
 680 deliver:
 681                 qdisc_bstats_update(sch, skb);
 682                 return skb;
 683         }
 684         skb = netem_peek(q);
 685         if (skb) {
 686                 u64 time_to_send;
 687                 u64 now = ktime_get_ns();
 688
 689                 /* if more time remaining? */
 690                 time_to_send = netem_skb_cb(skb)->time_to_send;
 691                 if (q->slot.slot_next && q->slot.slot_next < time_to_send)
 692                         get_slot_next(q, now);
 693
 694                 if (time_to_send <= now && q->slot.slot_next <= now) {
 695                         netem_erase_head(q, skb);
 696                         sch->q.qlen--;
 697                         qdisc_qstats_backlog_dec(sch, skb);
 698                         skb->next = NULL;
 699                         skb->prev = NULL;
 700                         /* skb->dev shares skb->rbnode area,
 701                          * we need to restore its value.
 702                          */
 703                         skb->dev = qdisc_dev(sch);
 704
 705                         if (q->slot.slot_next) {
 706                                 q->slot.packets_left--;
 707                                 q->slot.bytes_left -= qdisc_pkt_len(skb);
 708                                 if (q->slot.packets_left <= 0 ||
 709                                     q->slot.bytes_left <= 0)
 710                                         get_slot_next(q, now);
 711                         }
 712
 713                         if (q->qdisc) {
 714                                 unsigned int pkt_len = qdisc_pkt_len(skb);
 715                                 struct sk_buff *to_free = NULL;
 716                                 int err;
 717
 718                                 err = qdisc_enqueue(skb, q->qdisc, &to_free);
 719                                 kfree_skb_list(to_free);
 720                                 if (err != NET_XMIT_SUCCESS &&
 721                                     net_xmit_drop_count(err)) {
 722                                         qdisc_qstats_drop(sch);
 723                                         qdisc_tree_reduce_backlog(sch, 1,
 724                                                                   pkt_len);
 725                                 }
 726                                 goto tfifo_dequeue;
 727                         }
 728                         goto deliver;
 729                 }
 730
 731                 if (q->qdisc) {
 732                         skb = q->qdisc->ops->dequeue(q->qdisc);
 733                         if (skb)
 734                                 goto deliver;
 735                 }
 736
 737                 qdisc_watchdog_schedule_ns(&q->watchdog,
 738                                            max(time_to_send,
 739                                                q->slot.slot_next));
 740         }
 741
 742         if (q->qdisc) {
 743                 skb = q->qdisc->ops->dequeue(q->qdisc);
 744                 if (skb)
 745                         goto deliver;
 746         }
 747         return NULL;
 748 }
 749
 750 static void netem_reset(struct Qdisc *sch)
 751 {
 752         struct netem_sched_data *q = qdisc_priv(sch);
 753
 754         qdisc_reset_queue(sch);
 755         tfifo_reset(sch);
 756         if (q->qdisc)
 757                 qdisc_reset(q->qdisc);
 758         qdisc_watchdog_cancel(&q->watchdog);
 759 }
 760
 761 static void dist_free(struct disttable *d)
 762 {
 763         kvfree(d);
 764 }
 765
 766 /*
 767  * Distribution data is a variable size payload containing
 768  * signed 16 bit values.
 769  */
 770
 771 static int get_dist_table(struct Qdisc *sch, struct disttable **tbl,
 772                           const struct nlattr *attr)
 773 {
 774         size_t n = nla_len(attr)/sizeof(__s16);
 775         const __s16 *data = nla_data(attr);
 776         spinlock_t *root_lock;
 777         struct disttable *d;
 778         int i;
 779
 780         if (n > NETEM_DIST_MAX)
 781                 return -EINVAL;
 782
 783         d = kvmalloc(sizeof(struct disttable) + n * sizeof(s16), GFP_KERNEL);
 784         if (!d)
 785                 return -ENOMEM;
 786
 787         d->size = n;
 788         for (i = 0; i < n; i++)
 789                 d->table[i] = data[i];
 790
 791         root_lock = qdisc_root_sleeping_lock(sch);
 792
 793         spin_lock_bh(root_lock);
 794         swap(*tbl, d);
 795         spin_unlock_bh(root_lock);
 796
 797         dist_free(d);
 798         return 0;
 799 }
 800
 801 static void get_slot(struct netem_sched_data *q, const struct nlattr *attr)
 802 {
 803         const struct tc_netem_slot *c = nla_data(attr);
 804
 805         q->slot_config = *c;
 806         if (q->slot_config.max_packets == 0)
 807                 q->slot_config.max_packets = INT_MAX;
 808         if (q->slot_config.max_bytes == 0)
 809                 q->slot_config.max_bytes = INT_MAX;
 810         q->slot.packets_left = q->slot_config.max_packets;
 811         q->slot.bytes_left = q->slot_config.max_bytes;
 812         if (q->slot_config.min_delay | q->slot_config.max_delay |
 813             q->slot_config.dist_jitter)
 814                 q->slot.slot_next = ktime_get_ns();
 815         else
 816                 q->slot.slot_next = 0;
 817 }
 818
 819 static void get_correlation(struct netem_sched_data *q, const struct nlattr *attr)
 820 {
 821         const struct tc_netem_corr *c = nla_data(attr);
 822
 823         init_crandom(&q->delay_cor, c->delay_corr);
 824         init_crandom(&q->loss_cor, c->loss_corr);
 825         init_crandom(&q->dup_cor, c->dup_corr);
 826 }
 827
 828 static void get_reorder(struct netem_sched_data *q, const struct nlattr *attr)
 829 {
 830         const struct tc_netem_reorder *r = nla_data(attr);
 831
 832         q->reorder = r->probability;
 833         init_crandom(&q->reorder_cor, r->correlation);
 834 }
 835
 836 static void get_corrupt(struct netem_sched_data *q, const struct nlattr *attr)
 837 {
 838         const struct tc_netem_corrupt *r = nla_data(attr);
 839
 840         q->corrupt = r->probability;
 841         init_crandom(&q->corrupt_cor, r->correlation);
 842 }
 843
 844 static void get_rate(struct netem_sched_data *q, const struct nlattr *attr)
 845 {
 846         const struct tc_netem_rate *r = nla_data(attr);
 847
 848         q->rate = r->rate;
 849         q->packet_overhead = r->packet_overhead;
 850         q->cell_size = r->cell_size;
 851         q->cell_overhead = r->cell_overhead;
 852         if (q->cell_size)
 853                 q->cell_size_reciprocal = reciprocal_value(q->cell_size);
 854         else
 855                 q->cell_size_reciprocal = (struct reciprocal_value) { 0 };
 856 }
 857
 858 static int get_loss_clg(struct netem_sched_data *q, const struct nlattr *attr)
 859 {
 860         const struct nlattr *la;
 861         int rem;
 862
 863         nla_for_each_nested(la, attr, rem) {
 864                 u16 type = nla_type(la);
 865
 866                 switch (type) {
 867                 case NETEM_LOSS_GI: {
 868                         const struct tc_netem_gimodel *gi = nla_data(la);
 869
 870                         if (nla_len(la) < sizeof(struct tc_netem_gimodel)) {
 871                                 pr_info("netem: incorrect gi model size\n");
 872                                 return -EINVAL;
 873                         }
 874
 875                         q->loss_model = CLG_4_STATES;
 876
 877                         q->clg.state = TX_IN_GAP_PERIOD;
 878                         q->clg.a1 = gi->p13;
 879                         q->clg.a2 = gi->p31;
 880                         q->clg.a3 = gi->p32;
 881                         q->clg.a4 = gi->p14;
 882                         q->clg.a5 = gi->p23;
 883                         break;
 884                 }
 885
 886                 case NETEM_LOSS_GE: {
 887                         const struct tc_netem_gemodel *ge = nla_data(la);
 888
 889                         if (nla_len(la) < sizeof(struct tc_netem_gemodel)) {
 890                                 pr_info("netem: incorrect ge model size\n");
 891                                 return -EINVAL;
 892                         }
 893
 894                         q->loss_model = CLG_GILB_ELL;
 895                         q->clg.state = GOOD_STATE;
 896                         q->clg.a1 = ge->p;
 897                         q->clg.a2 = ge->r;
 898                         q->clg.a3 = ge->h;
 899                         q->clg.a4 = ge->k1;
 900                         break;
 901                 }
 902
 903                 default:
 904                         pr_info("netem: unknown loss type %u\n", type);
 905                         return -EINVAL;
 906                 }
 907         }
 908
 909         return 0;
 910 }
 911
 912 static const struct nla_policy netem_policy[TCA_NETEM_MAX + 1] = {
 913         [TCA_NETEM_CORR]        = { .len = sizeof(struct tc_netem_corr) },
 914         [TCA_NETEM_REORDER]     = { .len = sizeof(struct tc_netem_reorder) },
 915         [TCA_NETEM_CORRUPT]     = { .len = sizeof(struct tc_netem_corrupt) },
 916         [TCA_NETEM_RATE]        = { .len = sizeof(struct tc_netem_rate) },
 917         [TCA_NETEM_LOSS]        = { .type = NLA_NESTED },
 918         [TCA_NETEM_ECN]         = { .type = NLA_U32 },
 919         [TCA_NETEM_RATE64]      = { .type = NLA_U64 },
 920         [TCA_NETEM_LATENCY64]   = { .type = NLA_S64 },
 921         [TCA_NETEM_JITTER64]    = { .type = NLA_S64 },
 922         [TCA_NETEM_SLOT]        = { .len = sizeof(struct tc_netem_slot) },
 923 };
 924
 925 static int parse_attr(struct nlattr *tb[], int maxtype, struct nlattr *nla,
 926                       const struct nla_policy *policy, int len)
 927 {
 928         int nested_len = nla_len(nla) - NLA_ALIGN(len);
 929
 930         if (nested_len < 0) {
 931                 pr_info("netem: invalid attributes len %d\n", nested_len);
 932                 return -EINVAL;
 933         }
 934
 935         if (nested_len >= nla_attr_size(0))
 936                 return nla_parse_deprecated(tb, maxtype,
 937                                             nla_data(nla) + NLA_ALIGN(len),
 938                                             nested_len, policy, NULL);
 939
 940         memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
 941         return 0;
 942 }
 943
 944 /* Parse netlink message to set options */
 945 static int netem_change(struct Qdisc *sch, struct nlattr *opt,
 946                         struct netlink_ext_ack *extack)
 947 {
 948         struct netem_sched_data *q = qdisc_priv(sch);
 949         struct nlattr *tb[TCA_NETEM_MAX + 1];
 950         struct tc_netem_qopt *qopt;
 951         struct clgstate old_clg;
 952         int old_loss_model = CLG_RANDOM;
 953         int ret;
 954
 955         if (opt == NULL)
 956                 return -EINVAL;
 957
 958         qopt = nla_data(opt);
 959         ret = parse_attr(tb, TCA_NETEM_MAX, opt, netem_policy, sizeof(*qopt));
 960         if (ret < 0)
 961                 return ret;
 962
 963         /* backup q->clg and q->loss_model */
 964         old_clg = q->clg;
 965         old_loss_model = q->loss_model;
 966
 967         if (tb[TCA_NETEM_LOSS]) {
 968                 ret = get_loss_clg(q, tb[TCA_NETEM_LOSS]);
 969                 if (ret) {
 970                         q->loss_model = old_loss_model;
 971                         return ret;
 972                 }
 973         } else {
 974                 q->loss_model = CLG_RANDOM;
 975         }
 976
 977         if (tb[TCA_NETEM_DELAY_DIST]) {
 978                 ret = get_dist_table(sch, &q->delay_dist,
 979                                      tb[TCA_NETEM_DELAY_DIST]);
 980                 if (ret)
 981                         goto get_table_failure;
 982         }
 983
 984         if (tb[TCA_NETEM_SLOT_DIST]) {
 985                 ret = get_dist_table(sch, &q->slot_dist,
 986                                      tb[TCA_NETEM_SLOT_DIST]);
 987                 if (ret)
 988                         goto get_table_failure;
 989         }
 990
 991         sch->limit = qopt->limit;
 992
 993         q->latency = PSCHED_TICKS2NS(qopt->latency);
 994         q->jitter = PSCHED_TICKS2NS(qopt->jitter);
 995         q->limit = qopt->limit;
 996         q->gap = qopt->gap;
 997         q->counter = 0;
 998         q->loss = qopt->loss;
 999         q->duplicate = qopt->duplicate;
1000
1001         /* for compatibility with earlier versions.
1002          * if gap is set, need to assume 100% probability
1003          */
1004         if (q->gap)
1005                 q->reorder = ~0;
1006
1007         if (tb[TCA_NETEM_CORR])
1008                 get_correlation(q, tb[TCA_NETEM_CORR]);
1009
1010         if (tb[TCA_NETEM_REORDER])
1011                 get_reorder(q, tb[TCA_NETEM_REORDER]);
1012
1013         if (tb[TCA_NETEM_CORRUPT])
1014                 get_corrupt(q, tb[TCA_NETEM_CORRUPT]);
1015
1016         if (tb[TCA_NETEM_RATE])
1017                 get_rate(q, tb[TCA_NETEM_RATE]);
1018
1019         if (tb[TCA_NETEM_RATE64])
1020                 q->rate = max_t(u64, q->rate,
1021                                 nla_get_u64(tb[TCA_NETEM_RATE64]));
1022
1023         if (tb[TCA_NETEM_LATENCY64])
1024                 q->latency = nla_get_s64(tb[TCA_NETEM_LATENCY64]);
1025
1026         if (tb[TCA_NETEM_JITTER64])
1027                 q->jitter = nla_get_s64(tb[TCA_NETEM_JITTER64]);
1028
1029         if (tb[TCA_NETEM_ECN])
1030                 q->ecn = nla_get_u32(tb[TCA_NETEM_ECN]);
1031
1032         if (tb[TCA_NETEM_SLOT])
1033                 get_slot(q, tb[TCA_NETEM_SLOT]);
1034
1035         return ret;
1036
1037 get_table_failure:
1038         /* recover clg and loss_model, in case of
1039          * q->clg and q->loss_model were modified
1040          * in get_loss_clg()
1041          */
1042         q->clg = old_clg;
1043         q->loss_model = old_loss_model;
1044         return ret;
1045 }
1046
1047 static int netem_init(struct Qdisc *sch, struct nlattr *opt,
1048                       struct netlink_ext_ack *extack)
1049 {
1050         struct netem_sched_data *q = qdisc_priv(sch);
1051         int ret;
1052
1053         qdisc_watchdog_init(&q->watchdog, sch);
1054
1055         if (!opt)
1056                 return -EINVAL;
1057
1058         q->loss_model = CLG_RANDOM;
1059         ret = netem_change(sch, opt, extack);
1060         if (ret)
1061                 pr_info("netem: change failed\n");
1062         return ret;
1063 }
1064
1065 static void netem_destroy(struct Qdisc *sch)
1066 {
1067         struct netem_sched_data *q = qdisc_priv(sch);
1068
1069         qdisc_watchdog_cancel(&q->watchdog);
1070         if (q->qdisc)
1071                 qdisc_put(q->qdisc);
1072         dist_free(q->delay_dist);
1073         dist_free(q->slot_dist);
1074 }
1075
1076 static int dump_loss_model(const struct netem_sched_data *q,
1077                            struct sk_buff *skb)
1078 {
1079         struct nlattr *nest;
1080
1081         nest = nla_nest_start_noflag(skb, TCA_NETEM_LOSS);
1082         if (nest == NULL)
1083                 goto nla_put_failure;
1084
1085         switch (q->loss_model) {
1086         case CLG_RANDOM:
1087                 /* legacy loss model */
1088                 nla_nest_cancel(skb, nest);
1089                 return 0;       /* no data */
1090
1091         case CLG_4_STATES: {
1092                 struct tc_netem_gimodel gi = {
1093                         .p13 = q->clg.a1,
1094                         .p31 = q->clg.a2,
1095                         .p32 = q->clg.a3,
1096                         .p14 = q->clg.a4,
1097                         .p23 = q->clg.a5,
1098                 };
1099
1100                 if (nla_put(skb, NETEM_LOSS_GI, sizeof(gi), &gi))
1101                         goto nla_put_failure;
1102                 break;
1103         }
1104         case CLG_GILB_ELL: {
1105                 struct tc_netem_gemodel ge = {
1106                         .p = q->clg.a1,
1107                         .r = q->clg.a2,
1108                         .h = q->clg.a3,
1109                         .k1 = q->clg.a4,
1110                 };
1111
1112                 if (nla_put(skb, NETEM_LOSS_GE, sizeof(ge), &ge))
1113                         goto nla_put_failure;
1114                 break;
1115         }
1116         }
1117
1118         nla_nest_end(skb, nest);
1119         return 0;
1120
1121 nla_put_failure:
1122         nla_nest_cancel(skb, nest);
1123         return -1;
1124 }
1125
1126 static int netem_dump(struct Qdisc *sch, struct sk_buff *skb)
1127 {
1128         const struct netem_sched_data *q = qdisc_priv(sch);
1129         struct nlattr *nla = (struct nlattr *) skb_tail_pointer(skb);
1130         struct tc_netem_qopt qopt;
1131         struct tc_netem_corr cor;
1132         struct tc_netem_reorder reorder;
1133         struct tc_netem_corrupt corrupt;
1134         struct tc_netem_rate rate;
1135         struct tc_netem_slot slot;
1136
1137         qopt.latency = min_t(psched_tdiff_t, PSCHED_NS2TICKS(q->latency),
1138                              UINT_MAX);
1139         qopt.jitter = min_t(psched_tdiff_t, PSCHED_NS2TICKS(q->jitter),
1140                             UINT_MAX);
1141         qopt.limit = q->limit;
1142         qopt.loss = q->loss;
1143         qopt.gap = q->gap;
1144         qopt.duplicate = q->duplicate;
1145         if (nla_put(skb, TCA_OPTIONS, sizeof(qopt), &qopt))
1146                 goto nla_put_failure;
1147
1148         if (nla_put(skb, TCA_NETEM_LATENCY64, sizeof(q->latency), &q->latency))
1149                 goto nla_put_failure;
1150
1151         if (nla_put(skb, TCA_NETEM_JITTER64, sizeof(q->jitter), &q->jitter))
1152                 goto nla_put_failure;
1153
1154         cor.delay_corr = q->delay_cor.rho;
1155         cor.loss_corr = q->loss_cor.rho;
1156         cor.dup_corr = q->dup_cor.rho;
1157         if (nla_put(skb, TCA_NETEM_CORR, sizeof(cor), &cor))
1158                 goto nla_put_failure;
1159
1160         reorder.probability = q->reorder;
1161         reorder.correlation = q->reorder_cor.rho;
1162         if (nla_put(skb, TCA_NETEM_REORDER, sizeof(reorder), &reorder))
1163                 goto nla_put_failure;
1164
1165         corrupt.probability = q->corrupt;
1166         corrupt.correlation = q->corrupt_cor.rho;
1167         if (nla_put(skb, TCA_NETEM_CORRUPT, sizeof(corrupt), &corrupt))
1168                 goto nla_put_failure;
1169
1170         if (q->rate >= (1ULL << 32)) {
1171                 if (nla_put_u64_64bit(skb, TCA_NETEM_RATE64, q->rate,
1172                                       TCA_NETEM_PAD))
1173                         goto nla_put_failure;
1174                 rate.rate = ~0U;
1175         } else {
1176                 rate.rate = q->rate;
1177         }
1178         rate.packet_overhead = q->packet_overhead;
1179         rate.cell_size = q->cell_size;
1180         rate.cell_overhead = q->cell_overhead;
1181         if (nla_put(skb, TCA_NETEM_RATE, sizeof(rate), &rate))
1182                 goto nla_put_failure;
1183
1184         if (q->ecn && nla_put_u32(skb, TCA_NETEM_ECN, q->ecn))
1185                 goto nla_put_failure;
1186
1187         if (dump_loss_model(q, skb) != 0)
1188                 goto nla_put_failure;
1189
1190         if (q->slot_config.min_delay | q->slot_config.max_delay |
1191             q->slot_config.dist_jitter) {
1192                 slot = q->slot_config;
1193                 if (slot.max_packets == INT_MAX)
1194                         slot.max_packets = 0;
1195                 if (slot.max_bytes == INT_MAX)
1196                         slot.max_bytes = 0;
1197                 if (nla_put(skb, TCA_NETEM_SLOT, sizeof(slot), &slot))
1198                         goto nla_put_failure;
1199         }
1200
1201         return nla_nest_end(skb, nla);
1202
1203 nla_put_failure:
1204         nlmsg_trim(skb, nla);
1205         return -1;
1206 }
1207
1208 static int netem_dump_class(struct Qdisc *sch, unsigned long cl,
1209                           struct sk_buff *skb, struct tcmsg *tcm)
1210 {
1211         struct netem_sched_data *q = qdisc_priv(sch);
1212
1213         if (cl != 1 || !q->qdisc)       /* only one class */
1214                 return -ENOENT;
1215
1216         tcm->tcm_handle |= TC_H_MIN(1);
1217         tcm->tcm_info = q->qdisc->handle;
1218
1219         return 0;
1220 }
1221
1222 static int netem_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
1223                      struct Qdisc **old, struct netlink_ext_ack *extack)
1224 {
1225         struct netem_sched_data *q = qdisc_priv(sch);
1226
1227         *old = qdisc_replace(sch, new, &q->qdisc);
1228         return 0;
1229 }
1230
1231 static struct Qdisc *netem_leaf(struct Qdisc *sch, unsigned long arg)
1232 {
1233         struct netem_sched_data *q = qdisc_priv(sch);
1234         return q->qdisc;
1235 }
1236
1237 static unsigned long netem_find(struct Qdisc *sch, u32 classid)
1238 {
1239         return 1;
1240 }
1241
1242 static void netem_walk(struct Qdisc *sch, struct qdisc_walker *walker)
1243 {
1244         if (!walker->stop) {
1245                 if (walker->count >= walker->skip)
1246                         if (walker->fn(sch, 1, walker) < 0) {
1247                                 walker->stop = 1;
1248                                 return;
1249                         }
1250                 walker->count++;
1251         }
1252 }
1253
1254 static const struct Qdisc_class_ops netem_class_ops = {
1255         .graft          =       netem_graft,
1256         .leaf           =       netem_leaf,
1257         .find           =       netem_find,
1258         .walk           =       netem_walk,
1259         .dump           =       netem_dump_class,
1260 };
1261
1262 static struct Qdisc_ops netem_qdisc_ops __read_mostly = {
1263         .id             =       "netem",
1264         .cl_ops         =       &netem_class_ops,
1265         .priv_size      =       sizeof(struct netem_sched_data),
1266         .enqueue        =       netem_enqueue,
1267         .dequeue        =       netem_dequeue,
1268         .peek           =       qdisc_peek_dequeued,
1269         .init           =       netem_init,
1270         .reset          =       netem_reset,
1271         .destroy        =       netem_destroy,
1272         .change         =       netem_change,
1273         .dump           =       netem_dump,
1274         .owner          =       THIS_MODULE,
1275 };
1276
1277
1278 static int __init netem_module_init(void)
1279 {
1280         pr_info("netem: version " VERSION "\n");
1281         return register_qdisc(&netem_qdisc_ops);
1282 }
1283 static void __exit netem_module_exit(void)
1284 {
1285         unregister_qdisc(&netem_qdisc_ops);
1286 }
1287 module_init(netem_module_init)
1288 module_exit(netem_module_exit)
1289 MODULE_LICENSE("GPL");