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