net/sched/sch_netem.c

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