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