sys/netinet/ipfw/ip_dummynet.c

   1 /*-
   2  * Copyright (c) 1998-2002 Luigi Rizzo, Universita` di Pisa
   3  * Portions Copyright (c) 2000 Akamba Corp.
   4  * All rights reserved
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  */
  27
  28 #include <sys/cdefs.h>
  29 __FBSDID("$FreeBSD$");
  30
  31 #define DUMMYNET_DEBUG
  32
  33 #include "opt_inet6.h"
  34
  35 /*
  36  * This module implements IP dummynet, a bandwidth limiter/delay emulator
  37  * used in conjunction with the ipfw package.
  38  * Description of the data structures used is in ip_dummynet.h
  39  * Here you mainly find the following blocks of code:
  40  *  + variable declarations;
  41  *  + heap management functions;
  42  *  + scheduler and dummynet functions;
  43  *  + configuration and initialization.
  44  *
  45  * NOTA BENE: critical sections are protected by the "dummynet lock".
  46  *
  47  * Most important Changes:
  48  *
  49  * 011004: KLDable
  50  * 010124: Fixed WF2Q behaviour
  51  * 010122: Fixed spl protection.
  52  * 000601: WF2Q support
  53  * 000106: large rewrite, use heaps to handle very many pipes.
  54  * 980513:      initial release
  55  *
  56  * include files marked with XXX are probably not needed
  57  */
  58
  59 #include <sys/param.h>
  60 #include <sys/systm.h>
  61 #include <sys/malloc.h>
  62 #include <sys/mbuf.h>
  63 #include <sys/kernel.h>
  64 #include <sys/lock.h>
  65 #include <sys/module.h>
  66 #include <sys/priv.h>
  67 #include <sys/proc.h>
  68 #include <sys/rwlock.h>
  69 #include <sys/socket.h>
  70 #include <sys/socketvar.h>
  71 #include <sys/time.h>
  72 #include <sys/sysctl.h>
  73 #include <sys/taskqueue.h>
  74 #include <net/if.h>     /* IFNAMSIZ, struct ifaddr, ifq head, lock.h mutex.h */
  75 #include <net/netisr.h>
  76 #include <netinet/in.h>
  77 #include <netinet/ip.h>         /* ip_len, ip_off */
  78 #include <netinet/ip_fw.h>
  79 #include <netinet/ip_dummynet.h>
  80 #include <netinet/ip_var.h>     /* ip_output(), IP_FORWARDING */
  81
  82 #include <netinet/if_ether.h> /* various ether_* routines */
  83
  84 #include <netinet/ip6.h>       /* for ip6_input, ip6_output prototypes */
  85 #include <netinet6/ip6_var.h>
  86
  87 /*
  88  * We keep a private variable for the simulation time, but we could
  89  * probably use an existing one ("softticks" in sys/kern/kern_timeout.c)
  90  */
  91 static dn_key curr_time = 0 ; /* current simulation time */
  92
  93 static int dn_hash_size = 64 ;  /* default hash size */
  94
  95 /* statistics on number of queue searches and search steps */
  96 static long searches, search_steps ;
  97 static int pipe_expire = 1 ;   /* expire queue if empty */
  98 static int dn_max_ratio = 16 ; /* max queues/buckets ratio */
  99
 100 static long pipe_slot_limit = 100; /* Foot shooting limit for pipe queues. */
 101 static long pipe_byte_limit = 1024 * 1024;
 102
 103 static int red_lookup_depth = 256;      /* RED - default lookup table depth */
 104 static int red_avg_pkt_size = 512;      /* RED - default medium packet size */
 105 static int red_max_pkt_size = 1500;     /* RED - default max packet size */
 106
 107 static struct timeval prev_t, t;
 108 static long tick_last;                  /* Last tick duration (usec). */
 109 static long tick_delta;                 /* Last vs standard tick diff (usec). */
 110 static long tick_delta_sum;             /* Accumulated tick difference (usec).*/
 111 static long tick_adjustment;            /* Tick adjustments done. */
 112 static long tick_lost;                  /* Lost(coalesced) ticks number. */
 113 /* Adjusted vs non-adjusted curr_time difference (ticks). */
 114 static long tick_diff;
 115
 116 static int              io_fast;
 117 static unsigned long    io_pkt;
 118 static unsigned long    io_pkt_fast;
 119 static unsigned long    io_pkt_drop;
 120
 121 /*
 122  * Three heaps contain queues and pipes that the scheduler handles:
 123  *
 124  * ready_heap contains all dn_flow_queue related to fixed-rate pipes.
 125  *
 126  * wfq_ready_heap contains the pipes associated with WF2Q flows
 127  *
 128  * extract_heap contains pipes associated with delay lines.
 129  *
 130  */
 131
 132 MALLOC_DEFINE(M_DUMMYNET, "dummynet", "dummynet heap");
 133
 134 static struct dn_heap ready_heap, extract_heap, wfq_ready_heap ;
 135
 136 static int      heap_init(struct dn_heap *h, int size);
 137 static int      heap_insert (struct dn_heap *h, dn_key key1, void *p);
 138 static void     heap_extract(struct dn_heap *h, void *obj);
 139 static void     transmit_event(struct dn_pipe *pipe, struct mbuf **head,
 140                     struct mbuf **tail);
 141 static void     ready_event(struct dn_flow_queue *q, struct mbuf **head,
 142                     struct mbuf **tail);
 143 static void     ready_event_wfq(struct dn_pipe *p, struct mbuf **head,
 144                     struct mbuf **tail);
 145
 146 #define HASHSIZE        16
 147 #define HASH(num)       ((((num) >> 8) ^ ((num) >> 4) ^ (num)) & 0x0f)
 148 static struct dn_pipe_head      pipehash[HASHSIZE];     /* all pipes */
 149 static struct dn_flow_set_head  flowsethash[HASHSIZE];  /* all flowsets */
 150
 151 static struct callout dn_timeout;
 152
 153 extern  void (*bridge_dn_p)(struct mbuf *, struct ifnet *);
 154
 155 #ifdef SYSCTL_NODE
 156 SYSCTL_DECL(_net_inet);
 157 SYSCTL_DECL(_net_inet_ip);
 158
 159 SYSCTL_NODE(_net_inet_ip, OID_AUTO, dummynet, CTLFLAG_RW, 0, "Dummynet");
 160 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, hash_size,
 161     CTLFLAG_RW, &dn_hash_size, 0, "Default hash table size");
 162 #if 0   /* curr_time is 64 bit */
 163 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, curr_time,
 164     CTLFLAG_RD, &curr_time, 0, "Current tick");
 165 #endif
 166 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, ready_heap,
 167     CTLFLAG_RD, &ready_heap.size, 0, "Size of ready heap");
 168 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, extract_heap,
 169     CTLFLAG_RD, &extract_heap.size, 0, "Size of extract heap");
 170 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, searches,
 171     CTLFLAG_RD, &searches, 0, "Number of queue searches");
 172 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, search_steps,
 173     CTLFLAG_RD, &search_steps, 0, "Number of queue search steps");
 174 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, expire,
 175     CTLFLAG_RW, &pipe_expire, 0, "Expire queue if empty");
 176 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, max_chain_len,
 177     CTLFLAG_RW, &dn_max_ratio, 0,
 178     "Max ratio between dynamic queues and buckets");
 179 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, red_lookup_depth,
 180     CTLFLAG_RD, &red_lookup_depth, 0, "Depth of RED lookup table");
 181 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, red_avg_pkt_size,
 182     CTLFLAG_RD, &red_avg_pkt_size, 0, "RED Medium packet size");
 183 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, red_max_pkt_size,
 184     CTLFLAG_RD, &red_max_pkt_size, 0, "RED Max packet size");
 185 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, tick_delta,
 186     CTLFLAG_RD, &tick_delta, 0, "Last vs standard tick difference (usec).");
 187 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, tick_delta_sum,
 188     CTLFLAG_RD, &tick_delta_sum, 0, "Accumulated tick difference (usec).");
 189 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, tick_adjustment,
 190     CTLFLAG_RD, &tick_adjustment, 0, "Tick adjustments done.");
 191 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, tick_diff,
 192     CTLFLAG_RD, &tick_diff, 0,
 193     "Adjusted vs non-adjusted curr_time difference (ticks).");
 194 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, tick_lost,
 195     CTLFLAG_RD, &tick_lost, 0,
 196     "Number of ticks coalesced by dummynet taskqueue.");
 197 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, io_fast,
 198     CTLFLAG_RW, &io_fast, 0, "Enable fast dummynet io.");
 199 SYSCTL_ULONG(_net_inet_ip_dummynet, OID_AUTO, io_pkt,
 200     CTLFLAG_RD, &io_pkt, 0,
 201     "Number of packets passed to dummynet.");
 202 SYSCTL_ULONG(_net_inet_ip_dummynet, OID_AUTO, io_pkt_fast,
 203     CTLFLAG_RD, &io_pkt_fast, 0,
 204     "Number of packets bypassed dummynet scheduler.");
 205 SYSCTL_ULONG(_net_inet_ip_dummynet, OID_AUTO, io_pkt_drop,
 206     CTLFLAG_RD, &io_pkt_drop, 0,
 207     "Number of packets dropped by dummynet.");
 208 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, pipe_slot_limit,
 209     CTLFLAG_RW, &pipe_slot_limit, 0, "Upper limit in slots for pipe queue.");
 210 SYSCTL_LONG(_net_inet_ip_dummynet, OID_AUTO, pipe_byte_limit,
 211     CTLFLAG_RW, &pipe_byte_limit, 0, "Upper limit in bytes for pipe queue.");
 212 #endif
 213
 214 #ifdef DUMMYNET_DEBUG
 215 int     dummynet_debug = 0;
 216 #ifdef SYSCTL_NODE
 217 SYSCTL_INT(_net_inet_ip_dummynet, OID_AUTO, debug, CTLFLAG_RW, &dummynet_debug,
 218             0, "control debugging printfs");
 219 #endif
 220 #define DPRINTF(X)      if (dummynet_debug) printf X
 221 #else
 222 #define DPRINTF(X)
 223 #endif
 224
 225 static struct task      dn_task;
 226 static struct taskqueue *dn_tq = NULL;
 227 static void dummynet_task(void *, int);
 228
 229 static struct mtx dummynet_mtx;
 230 #define DUMMYNET_LOCK_INIT() \
 231         mtx_init(&dummynet_mtx, "dummynet", NULL, MTX_DEF)
 232 #define DUMMYNET_LOCK_DESTROY() mtx_destroy(&dummynet_mtx)
 233 #define DUMMYNET_LOCK()         mtx_lock(&dummynet_mtx)
 234 #define DUMMYNET_UNLOCK()       mtx_unlock(&dummynet_mtx)
 235 #define DUMMYNET_LOCK_ASSERT()  mtx_assert(&dummynet_mtx, MA_OWNED)
 236
 237 static int      config_pipe(struct dn_pipe *p);
 238 static int      ip_dn_ctl(struct sockopt *sopt);
 239
 240 static void     dummynet(void *);
 241 static void     dummynet_flush(void);
 242 static void     dummynet_send(struct mbuf *);
 243 void            dummynet_drain(void);
 244 static int      dummynet_io(struct mbuf **, int , struct ip_fw_args *);
 245
 246 /*
 247  * Heap management functions.
 248  *
 249  * In the heap, first node is element 0. Children of i are 2i+1 and 2i+2.
 250  * Some macros help finding parent/children so we can optimize them.
 251  *
 252  * heap_init() is called to expand the heap when needed.
 253  * Increment size in blocks of 16 entries.
 254  * XXX failure to allocate a new element is a pretty bad failure
 255  * as we basically stall a whole queue forever!!
 256  * Returns 1 on error, 0 on success
 257  */
 258 #define HEAP_FATHER(x) ( ( (x) - 1 ) / 2 )
 259 #define HEAP_LEFT(x) ( 2*(x) + 1 )
 260 #define HEAP_IS_LEFT(x) ( (x) & 1 )
 261 #define HEAP_RIGHT(x) ( 2*(x) + 2 )
 262 #define HEAP_SWAP(a, b, buffer) { buffer = a ; a = b ; b = buffer ; }
 263 #define HEAP_INCREMENT  15
 264
 265 static int
 266 heap_init(struct dn_heap *h, int new_size)
 267 {
 268     struct dn_heap_entry *p;
 269
 270     if (h->size >= new_size ) {
 271         printf("dummynet: %s, Bogus call, have %d want %d\n", __func__,
 272                 h->size, new_size);
 273         return 0 ;
 274     }
 275     new_size = (new_size + HEAP_INCREMENT ) & ~HEAP_INCREMENT ;
 276     p = malloc(new_size * sizeof(*p), M_DUMMYNET, M_NOWAIT);
 277     if (p == NULL) {
 278         printf("dummynet: %s, resize %d failed\n", __func__, new_size );
 279         return 1 ; /* error */
 280     }
 281     if (h->size > 0) {
 282         bcopy(h->p, p, h->size * sizeof(*p) );
 283         free(h->p, M_DUMMYNET);
 284     }
 285     h->p = p ;
 286     h->size = new_size ;
 287     return 0 ;
 288 }
 289
 290 /*
 291  * Insert element in heap. Normally, p != NULL, we insert p in
 292  * a new position and bubble up. If p == NULL, then the element is
 293  * already in place, and key is the position where to start the
 294  * bubble-up.
 295  * Returns 1 on failure (cannot allocate new heap entry)
 296  *
 297  * If offset > 0 the position (index, int) of the element in the heap is
 298  * also stored in the element itself at the given offset in bytes.
 299  */
 300 #define SET_OFFSET(heap, node) \
 301     if (heap->offset > 0) \
 302             *((int *)((char *)(heap->p[node].object) + heap->offset)) = node ;
 303 /*
 304  * RESET_OFFSET is used for sanity checks. It sets offset to an invalid value.
 305  */
 306 #define RESET_OFFSET(heap, node) \
 307     if (heap->offset > 0) \
 308             *((int *)((char *)(heap->p[node].object) + heap->offset)) = -1 ;
 309 static int
 310 heap_insert(struct dn_heap *h, dn_key key1, void *p)
 311 {
 312     int son = h->elements ;
 313
 314     if (p == NULL)      /* data already there, set starting point */
 315         son = key1 ;
 316     else {              /* insert new element at the end, possibly resize */
 317         son = h->elements ;
 318         if (son == h->size) /* need resize... */
 319             if (heap_init(h, h->elements+1) )
 320                 return 1 ; /* failure... */
 321         h->p[son].object = p ;
 322         h->p[son].key = key1 ;
 323         h->elements++ ;
 324     }
 325     while (son > 0) {                           /* bubble up */
 326         int father = HEAP_FATHER(son) ;
 327         struct dn_heap_entry tmp  ;
 328
 329         if (DN_KEY_LT( h->p[father].key, h->p[son].key ) )
 330             break ; /* found right position */
 331         /* son smaller than father, swap and repeat */
 332         HEAP_SWAP(h->p[son], h->p[father], tmp) ;
 333         SET_OFFSET(h, son);
 334         son = father ;
 335     }
 336     SET_OFFSET(h, son);
 337     return 0 ;
 338 }
 339
 340 /*
 341  * remove top element from heap, or obj if obj != NULL
 342  */
 343 static void
 344 heap_extract(struct dn_heap *h, void *obj)
 345 {
 346     int child, father, max = h->elements - 1 ;
 347
 348     if (max < 0) {
 349         printf("dummynet: warning, extract from empty heap 0x%p\n", h);
 350         return ;
 351     }
 352     father = 0 ; /* default: move up smallest child */
 353     if (obj != NULL) { /* extract specific element, index is at offset */
 354         if (h->offset <= 0)
 355             panic("dummynet: heap_extract from middle not supported on this heap!!!\n");
 356         father = *((int *)((char *)obj + h->offset)) ;
 357         if (father < 0 || father >= h->elements) {
 358             printf("dummynet: heap_extract, father %d out of bound 0..%d\n",
 359                 father, h->elements);
 360             panic("dummynet: heap_extract");
 361         }
 362     }
 363     RESET_OFFSET(h, father);
 364     child = HEAP_LEFT(father) ;         /* left child */
 365     while (child <= max) {              /* valid entry */
 366         if (child != max && DN_KEY_LT(h->p[child+1].key, h->p[child].key) )
 367             child = child+1 ;           /* take right child, otherwise left */
 368         h->p[father] = h->p[child] ;
 369         SET_OFFSET(h, father);
 370         father = child ;
 371         child = HEAP_LEFT(child) ;   /* left child for next loop */
 372     }
 373     h->elements-- ;
 374     if (father != max) {
 375         /*
 376          * Fill hole with last entry and bubble up, reusing the insert code
 377          */
 378         h->p[father] = h->p[max] ;
 379         heap_insert(h, father, NULL); /* this one cannot fail */
 380     }
 381 }
 382
 383 #if 0
 384 /*
 385  * change object position and update references
 386  * XXX this one is never used!
 387  */
 388 static void
 389 heap_move(struct dn_heap *h, dn_key new_key, void *object)
 390 {
 391     int temp;
 392     int i ;
 393     int max = h->elements-1 ;
 394     struct dn_heap_entry buf ;
 395
 396     if (h->offset <= 0)
 397         panic("cannot move items on this heap");
 398
 399     i = *((int *)((char *)object + h->offset));
 400     if (DN_KEY_LT(new_key, h->p[i].key) ) { /* must move up */
 401         h->p[i].key = new_key ;
 402         for (; i>0 && DN_KEY_LT(new_key, h->p[(temp = HEAP_FATHER(i))].key) ;
 403                  i = temp ) { /* bubble up */
 404             HEAP_SWAP(h->p[i], h->p[temp], buf) ;
 405             SET_OFFSET(h, i);
 406         }
 407     } else {            /* must move down */
 408         h->p[i].key = new_key ;
 409         while ( (temp = HEAP_LEFT(i)) <= max ) { /* found left child */
 410             if ((temp != max) && DN_KEY_GT(h->p[temp].key, h->p[temp+1].key))
 411                 temp++ ; /* select child with min key */
 412             if (DN_KEY_GT(new_key, h->p[temp].key)) { /* go down */
 413                 HEAP_SWAP(h->p[i], h->p[temp], buf) ;
 414                 SET_OFFSET(h, i);
 415             } else
 416                 break ;
 417             i = temp ;
 418         }
 419     }
 420     SET_OFFSET(h, i);
 421 }
 422 #endif /* heap_move, unused */
 423
 424 /*
 425  * heapify() will reorganize data inside an array to maintain the
 426  * heap property. It is needed when we delete a bunch of entries.
 427  */
 428 static void
 429 heapify(struct dn_heap *h)
 430 {
 431     int i ;
 432
 433     for (i = 0 ; i < h->elements ; i++ )
 434         heap_insert(h, i , NULL) ;
 435 }
 436
 437 /*
 438  * cleanup the heap and free data structure
 439  */
 440 static void
 441 heap_free(struct dn_heap *h)
 442 {
 443     if (h->size >0 )
 444         free(h->p, M_DUMMYNET);
 445     bzero(h, sizeof(*h) );
 446 }
 447
 448 /*
 449  * --- end of heap management functions ---
 450  */
 451
 452 /*
 453  * Return the mbuf tag holding the dummynet state.  As an optimization
 454  * this is assumed to be the first tag on the list.  If this turns out
 455  * wrong we'll need to search the list.
 456  */
 457 static struct dn_pkt_tag *
 458 dn_tag_get(struct mbuf *m)
 459 {
 460     struct m_tag *mtag = m_tag_first(m);
 461     KASSERT(mtag != NULL &&
 462             mtag->m_tag_cookie == MTAG_ABI_COMPAT &&
 463             mtag->m_tag_id == PACKET_TAG_DUMMYNET,
 464             ("packet on dummynet queue w/o dummynet tag!"));
 465     return (struct dn_pkt_tag *)(mtag+1);
 466 }
 467
 468 /*
 469  * Scheduler functions:
 470  *
 471  * transmit_event() is called when the delay-line needs to enter
 472  * the scheduler, either because of existing pkts getting ready,
 473  * or new packets entering the queue. The event handled is the delivery
 474  * time of the packet.
 475  *
 476  * ready_event() does something similar with fixed-rate queues, and the
 477  * event handled is the finish time of the head pkt.
 478  *
 479  * wfq_ready_event() does something similar with WF2Q queues, and the
 480  * event handled is the start time of the head pkt.
 481  *
 482  * In all cases, we make sure that the data structures are consistent
 483  * before passing pkts out, because this might trigger recursive
 484  * invocations of the procedures.
 485  */
 486 static void
 487 transmit_event(struct dn_pipe *pipe, struct mbuf **head, struct mbuf **tail)
 488 {
 489         struct mbuf *m;
 490         struct dn_pkt_tag *pkt;
 491
 492         DUMMYNET_LOCK_ASSERT();
 493
 494         while ((m = pipe->head) != NULL) {
 495                 pkt = dn_tag_get(m);
 496                 if (!DN_KEY_LEQ(pkt->output_time, curr_time))
 497                         break;
 498
 499                 pipe->head = m->m_nextpkt;
 500                 if (*tail != NULL)
 501                         (*tail)->m_nextpkt = m;
 502                 else
 503                         *head = m;
 504                 *tail = m;
 505         }
 506         if (*tail != NULL)
 507                 (*tail)->m_nextpkt = NULL;
 508
 509         /* If there are leftover packets, put into the heap for next event. */
 510         if ((m = pipe->head) != NULL) {
 511                 pkt = dn_tag_get(m);
 512                 /*
 513                  * XXX Should check errors on heap_insert, by draining the
 514                  * whole pipe p and hoping in the future we are more successful.
 515                  */
 516                 heap_insert(&extract_heap, pkt->output_time, pipe);
 517         }
 518 }
 519
 520 #define div64(a, b)     ((int64_t)(a) / (int64_t)(b))
 521 #define DN_TO_DROP      0xffff
 522 /*
 523  * Compute how many ticks we have to wait before being able to send
 524  * a packet. This is computed as the "wire time" for the packet
 525  * (length + extra bits), minus the credit available, scaled to ticks.
 526  * Check that the result is not be negative (it could be if we have
 527  * too much leftover credit in q->numbytes).
 528  */
 529 static inline dn_key
 530 set_ticks(struct mbuf *m, struct dn_flow_queue *q, struct dn_pipe *p)
 531 {
 532         int64_t ret;
 533
 534         ret = div64( (m->m_pkthdr.len * 8 + q->extra_bits) * hz
 535                 - q->numbytes + p->bandwidth - 1 , p->bandwidth);
 536 #if 0
 537         printf("%s %d extra_bits %d numb %d ret %d\n",
 538                 __FUNCTION__, __LINE__,
 539                 (int)(q->extra_bits & 0xffffffff),
 540                 (int)(q->numbytes & 0xffffffff),
 541                 (int)(ret & 0xffffffff));
 542 #endif
 543         if (ret < 0)
 544                 ret = 0;
 545         return ret;
 546 }
 547
 548 /*
 549  * Convert the additional MAC overheads/delays into an equivalent
 550  * number of bits for the given data rate. The samples are in milliseconds
 551  * so we need to divide by 1000.
 552  */
 553 static dn_key
 554 compute_extra_bits(struct mbuf *pkt, struct dn_pipe *p)
 555 {
 556         int index;
 557         dn_key extra_bits;
 558
 559         if (!p->samples || p->samples_no == 0)
 560                 return 0;
 561         index  = random() % p->samples_no;
 562         extra_bits = ((dn_key)p->samples[index] * p->bandwidth) / 1000;
 563         if (index >= p->loss_level) {
 564                 struct dn_pkt_tag *dt = dn_tag_get(pkt);
 565                 if (dt)
 566                         dt->dn_dir = DN_TO_DROP;
 567         }
 568         return extra_bits;
 569 }
 570
 571 static void
 572 free_pipe(struct dn_pipe *p)
 573 {
 574         if (p->samples)
 575                 free(p->samples, M_DUMMYNET);
 576         free(p, M_DUMMYNET);
 577 }
 578
 579 /*
 580  * extract pkt from queue, compute output time (could be now)
 581  * and put into delay line (p_queue)
 582  */
 583 static void
 584 move_pkt(struct mbuf *pkt, struct dn_flow_queue *q, struct dn_pipe *p,
 585     int len)
 586 {
 587     struct dn_pkt_tag *dt = dn_tag_get(pkt);
 588
 589     q->head = pkt->m_nextpkt ;
 590     q->len-- ;
 591     q->len_bytes -= len ;
 592
 593     dt->output_time = curr_time + p->delay ;
 594
 595     if (p->head == NULL)
 596         p->head = pkt;
 597     else
 598         p->tail->m_nextpkt = pkt;
 599     p->tail = pkt;
 600     p->tail->m_nextpkt = NULL;
 601 }
 602
 603 /*
 604  * ready_event() is invoked every time the queue must enter the
 605  * scheduler, either because the first packet arrives, or because
 606  * a previously scheduled event fired.
 607  * On invokation, drain as many pkts as possible (could be 0) and then
 608  * if there are leftover packets reinsert the pkt in the scheduler.
 609  */
 610 static void
 611 ready_event(struct dn_flow_queue *q, struct mbuf **head, struct mbuf **tail)
 612 {
 613         struct mbuf *pkt;
 614         struct dn_pipe *p = q->fs->pipe;
 615         int p_was_empty;
 616
 617         DUMMYNET_LOCK_ASSERT();
 618
 619         if (p == NULL) {
 620                 printf("dummynet: ready_event- pipe is gone\n");
 621                 return;
 622         }
 623         p_was_empty = (p->head == NULL);
 624
 625         /*
 626          * Schedule fixed-rate queues linked to this pipe:
 627          * account for the bw accumulated since last scheduling, then
 628          * drain as many pkts as allowed by q->numbytes and move to
 629          * the delay line (in p) computing output time.
 630          * bandwidth==0 (no limit) means we can drain the whole queue,
 631          * setting len_scaled = 0 does the job.
 632          */
 633         q->numbytes += (curr_time - q->sched_time) * p->bandwidth;
 634         while ((pkt = q->head) != NULL) {
 635                 int len = pkt->m_pkthdr.len;
 636                 dn_key len_scaled = p->bandwidth ? len*8*hz
 637                         + q->extra_bits*hz
 638                         : 0;
 639
 640                 if (DN_KEY_GT(len_scaled, q->numbytes))
 641                         break;
 642                 q->numbytes -= len_scaled;
 643                 move_pkt(pkt, q, p, len);
 644                 if (q->head)
 645                         q->extra_bits = compute_extra_bits(q->head, p);
 646         }
 647         /*
 648          * If we have more packets queued, schedule next ready event
 649          * (can only occur when bandwidth != 0, otherwise we would have
 650          * flushed the whole queue in the previous loop).
 651          * To this purpose we record the current time and compute how many
 652          * ticks to go for the finish time of the packet.
 653          */
 654         if ((pkt = q->head) != NULL) {  /* this implies bandwidth != 0 */
 655                 dn_key t = set_ticks(pkt, q, p); /* ticks i have to wait */
 656
 657                 q->sched_time = curr_time;
 658                 heap_insert(&ready_heap, curr_time + t, (void *)q);
 659                 /*
 660                  * XXX Should check errors on heap_insert, and drain the whole
 661                  * queue on error hoping next time we are luckier.
 662                  */
 663         } else          /* RED needs to know when the queue becomes empty. */
 664                 q->idle_time = curr_time;
 665
 666         /*
 667          * If the delay line was empty call transmit_event() now.
 668          * Otherwise, the scheduler will take care of it.
 669          */
 670         if (p_was_empty)
 671                 transmit_event(p, head, tail);
 672 }
 673
 674 /*
 675  * Called when we can transmit packets on WF2Q queues. Take pkts out of
 676  * the queues at their start time, and enqueue into the delay line.
 677  * Packets are drained until p->numbytes < 0. As long as
 678  * len_scaled >= p->numbytes, the packet goes into the delay line
 679  * with a deadline p->delay. For the last packet, if p->numbytes < 0,
 680  * there is an additional delay.
 681  */
 682 static void
 683 ready_event_wfq(struct dn_pipe *p, struct mbuf **head, struct mbuf **tail)
 684 {
 685         int p_was_empty = (p->head == NULL);
 686         struct dn_heap *sch = &(p->scheduler_heap);
 687         struct dn_heap *neh = &(p->not_eligible_heap);
 688
 689         DUMMYNET_LOCK_ASSERT();
 690
 691         if (p->if_name[0] == 0)         /* tx clock is simulated */
 692                 p->numbytes += (curr_time - p->sched_time) * p->bandwidth;
 693         else {  /*
 694                  * tx clock is for real,
 695                  * the ifq must be empty or this is a NOP.
 696                  */
 697                 if (p->ifp && p->ifp->if_snd.ifq_head != NULL)
 698                         return;
 699                 else {
 700                         DPRINTF(("dummynet: pipe %d ready from %s --\n",
 701                             p->pipe_nr, p->if_name));
 702                 }
 703         }
 704
 705         /*
 706          * While we have backlogged traffic AND credit, we need to do
 707          * something on the queue.
 708          */
 709         while (p->numbytes >= 0 && (sch->elements > 0 || neh->elements > 0)) {
 710                 if (sch->elements > 0) {
 711                         /* Have some eligible pkts to send out. */
 712                         struct dn_flow_queue *q = sch->p[0].object;
 713                         struct mbuf *pkt = q->head;
 714                         struct dn_flow_set *fs = q->fs;
 715                         uint64_t len = pkt->m_pkthdr.len;
 716                         int len_scaled = p->bandwidth ? len * 8 * hz : 0;
 717
 718                         heap_extract(sch, NULL); /* Remove queue from heap. */
 719                         p->numbytes -= len_scaled;
 720                         move_pkt(pkt, q, p, len);
 721
 722                         p->V += (len << MY_M) / p->sum; /* Update V. */
 723                         q->S = q->F;                    /* Update start time. */
 724                         if (q->len == 0) {
 725                                 /* Flow not backlogged any more. */
 726                                 fs->backlogged--;
 727                                 heap_insert(&(p->idle_heap), q->F, q);
 728                         } else {
 729                                 /* Still backlogged. */
 730
 731                                 /*
 732                                  * Update F and position in backlogged queue,
 733                                  * then put flow in not_eligible_heap
 734                                  * (we will fix this later).
 735                                  */
 736                                 len = (q->head)->m_pkthdr.len;
 737                                 q->F += (len << MY_M) / (uint64_t)fs->weight;
 738                                 if (DN_KEY_LEQ(q->S, p->V))
 739                                         heap_insert(neh, q->S, q);
 740                                 else
 741                                         heap_insert(sch, q->F, q);
 742                         }
 743                 }
 744                 /*
 745                  * Now compute V = max(V, min(S_i)). Remember that all elements
 746                  * in sch have by definition S_i <= V so if sch is not empty,
 747                  * V is surely the max and we must not update it. Conversely,
 748                  * if sch is empty we only need to look at neh.
 749                  */
 750                 if (sch->elements == 0 && neh->elements > 0)
 751                         p->V = MAX64(p->V, neh->p[0].key);
 752                 /* Move from neh to sch any packets that have become eligible */
 753                 while (neh->elements > 0 && DN_KEY_LEQ(neh->p[0].key, p->V)) {
 754                         struct dn_flow_queue *q = neh->p[0].object;
 755                         heap_extract(neh, NULL);
 756                         heap_insert(sch, q->F, q);
 757                 }
 758
 759                 if (p->if_name[0] != '\0') { /* Tx clock is from a real thing */
 760                         p->numbytes = -1;       /* Mark not ready for I/O. */
 761                         break;
 762                 }
 763         }
 764         if (sch->elements == 0 && neh->elements == 0 && p->numbytes >= 0) {
 765                 p->idle_time = curr_time;
 766                 /*
 767                  * No traffic and no events scheduled.
 768                  * We can get rid of idle-heap.
 769                  */
 770                 if (p->idle_heap.elements > 0) {
 771                         int i;
 772
 773                         for (i = 0; i < p->idle_heap.elements; i++) {
 774                                 struct dn_flow_queue *q;
 775
 776                                 q = p->idle_heap.p[i].object;
 777                                 q->F = 0;
 778                                 q->S = q->F + 1;
 779                         }
 780                         p->sum = 0;
 781                         p->V = 0;
 782                         p->idle_heap.elements = 0;
 783                 }
 784         }
 785         /*
 786          * If we are getting clocks from dummynet (not a real interface) and
 787          * If we are under credit, schedule the next ready event.
 788          * Also fix the delivery time of the last packet.
 789          */
 790         if (p->if_name[0]==0 && p->numbytes < 0) { /* This implies bw > 0. */
 791                 dn_key t = 0;           /* Number of ticks i have to wait. */
 792
 793                 if (p->bandwidth > 0)
 794                         t = (p->bandwidth - 1 - p->numbytes) / p->bandwidth;
 795                 dn_tag_get(p->tail)->output_time += t;
 796                 p->sched_time = curr_time;
 797                 heap_insert(&wfq_ready_heap, curr_time + t, (void *)p);
 798                 /*
 799                  * XXX Should check errors on heap_insert, and drain the whole
 800                  * queue on error hoping next time we are luckier.
 801                  */
 802         }
 803
 804         /*
 805          * If the delay line was empty call transmit_event() now.
 806          * Otherwise, the scheduler will take care of it.
 807          */
 808         if (p_was_empty)
 809                 transmit_event(p, head, tail);
 810 }
 811
 812 /*
 813  * This is called one tick, after previous run. It is used to
 814  * schedule next run.
 815  */
 816 static void
 817 dummynet(void * __unused unused)
 818 {
 819
 820         taskqueue_enqueue(dn_tq, &dn_task);
 821 }
 822
 823 /*
 824  * The main dummynet processing function.
 825  */
 826 static void
 827 dummynet_task(void *context, int pending)
 828 {
 829         struct mbuf *head = NULL, *tail = NULL;
 830         struct dn_pipe *pipe;
 831         struct dn_heap *heaps[3];
 832         struct dn_heap *h;
 833         void *p;        /* generic parameter to handler */
 834         int i;
 835
 836         DUMMYNET_LOCK();
 837
 838         heaps[0] = &ready_heap;                 /* fixed-rate queues */
 839         heaps[1] = &wfq_ready_heap;             /* wfq queues */
 840         heaps[2] = &extract_heap;               /* delay line */
 841
 842         /* Update number of lost(coalesced) ticks. */
 843         tick_lost += pending - 1;
 844
 845         getmicrouptime(&t);
 846         /* Last tick duration (usec). */
 847         tick_last = (t.tv_sec - prev_t.tv_sec) * 1000000 +
 848             (t.tv_usec - prev_t.tv_usec);
 849         /* Last tick vs standard tick difference (usec). */
 850         tick_delta = (tick_last * hz - 1000000) / hz;
 851         /* Accumulated tick difference (usec). */
 852         tick_delta_sum += tick_delta;
 853
 854         prev_t = t;
 855
 856         /*
 857          * Adjust curr_time if accumulated tick difference greater than
 858          * 'standard' tick. Since curr_time should be monotonically increasing,
 859          * we do positive adjustment as required and throttle curr_time in
 860          * case of negative adjustment.
 861          */
 862         curr_time++;
 863         if (tick_delta_sum - tick >= 0) {
 864                 int diff = tick_delta_sum / tick;
 865
 866                 curr_time += diff;
 867                 tick_diff += diff;
 868                 tick_delta_sum %= tick;
 869                 tick_adjustment++;
 870         } else if (tick_delta_sum + tick <= 0) {
 871                 curr_time--;
 872                 tick_diff--;
 873                 tick_delta_sum += tick;
 874                 tick_adjustment++;
 875         }
 876
 877         for (i = 0; i < 3; i++) {
 878                 h = heaps[i];
 879                 while (h->elements > 0 && DN_KEY_LEQ(h->p[0].key, curr_time)) {
 880                         if (h->p[0].key > curr_time)
 881                                 printf("dummynet: warning, "
 882                                     "heap %d is %d ticks late\n",
 883                                     i, (int)(curr_time - h->p[0].key));
 884                         /* store a copy before heap_extract */
 885                         p = h->p[0].object;
 886                         /* need to extract before processing */
 887                         heap_extract(h, NULL);
 888                         if (i == 0)
 889                                 ready_event(p, &head, &tail);
 890                         else if (i == 1) {
 891                                 struct dn_pipe *pipe = p;
 892                                 if (pipe->if_name[0] != '\0')
 893                                         printf("dummynet: bad ready_event_wfq "
 894                                             "for pipe %s\n", pipe->if_name);
 895                                 else
 896                                         ready_event_wfq(p, &head, &tail);
 897                         } else
 898                                 transmit_event(p, &head, &tail);
 899                 }
 900         }
 901
 902         /* Sweep pipes trying to expire idle flow_queues. */
 903         for (i = 0; i < HASHSIZE; i++)
 904                 SLIST_FOREACH(pipe, &pipehash[i], next)
 905                         if (pipe->idle_heap.elements > 0 &&
 906                             DN_KEY_LT(pipe->idle_heap.p[0].key, pipe->V)) {
 907                                 struct dn_flow_queue *q =
 908                                     pipe->idle_heap.p[0].object;
 909
 910                                 heap_extract(&(pipe->idle_heap), NULL);
 911                                 /* Mark timestamp as invalid. */
 912                                 q->S = q->F + 1;
 913                                 pipe->sum -= q->fs->weight;
 914                         }
 915
 916         DUMMYNET_UNLOCK();
 917
 918         if (head != NULL)
 919                 dummynet_send(head);
 920
 921         callout_reset(&dn_timeout, 1, dummynet, NULL);
 922 }
 923
 924 static void
 925 dummynet_send(struct mbuf *m)
 926 {
 927         struct dn_pkt_tag *pkt;
 928         struct mbuf *n;
 929         struct ip *ip;
 930
 931         for (; m != NULL; m = n) {
 932                 n = m->m_nextpkt;
 933                 m->m_nextpkt = NULL;
 934                 pkt = dn_tag_get(m);
 935                 switch (pkt->dn_dir) {
 936                 case DN_TO_IP_OUT:
 937                         ip_output(m, NULL, NULL, IP_FORWARDING, NULL, NULL);
 938                         break ;
 939                 case DN_TO_IP_IN :
 940                         ip = mtod(m, struct ip *);
 941                         ip->ip_len = htons(ip->ip_len);
 942                         ip->ip_off = htons(ip->ip_off);
 943                         netisr_dispatch(NETISR_IP, m);
 944                         break;
 945 #ifdef INET6
 946                 case DN_TO_IP6_IN:
 947                         netisr_dispatch(NETISR_IPV6, m);
 948                         break;
 949
 950                 case DN_TO_IP6_OUT:
 951                         ip6_output(m, NULL, NULL, IPV6_FORWARDING, NULL, NULL, NULL);
 952                         break;
 953 #endif
 954                 case DN_TO_IFB_FWD:
 955                         if (bridge_dn_p != NULL)
 956                                 ((*bridge_dn_p)(m, pkt->ifp));
 957                         else
 958                                 printf("dummynet: if_bridge not loaded\n");
 959
 960                         break;
 961                 case DN_TO_ETH_DEMUX:
 962                         /*
 963                          * The Ethernet code assumes the Ethernet header is
 964                          * contiguous in the first mbuf header.
 965                          * Insure this is true.
 966                          */
 967                         if (m->m_len < ETHER_HDR_LEN &&
 968                             (m = m_pullup(m, ETHER_HDR_LEN)) == NULL) {
 969                                 printf("dummynet/ether: pullup failed, "
 970                                     "dropping packet\n");
 971                                 break;
 972                         }
 973                         ether_demux(m->m_pkthdr.rcvif, m);
 974                         break;
 975                 case DN_TO_ETH_OUT:
 976                         ether_output_frame(pkt->ifp, m);
 977                         break;
 978
 979                 case DN_TO_DROP:
 980                         /* drop the packet after some time */
 981                         m_freem(m);
 982                         break;
 983
 984                 default:
 985                         printf("dummynet: bad switch %d!\n", pkt->dn_dir);
 986                         m_freem(m);
 987                         break;
 988                 }
 989         }
 990 }
 991
 992 /*
 993  * Unconditionally expire empty queues in case of shortage.
 994  * Returns the number of queues freed.
 995  */
 996 static int
 997 expire_queues(struct dn_flow_set *fs)
 998 {
 999     struct dn_flow_queue *q, *prev ;
1000     int i, initial_elements = fs->rq_elements ;
1001
1002     if (fs->last_expired == time_uptime)
1003         return 0 ;
1004     fs->last_expired = time_uptime ;
1005     for (i = 0 ; i <= fs->rq_size ; i++) /* last one is overflow */
1006         for (prev=NULL, q = fs->rq[i] ; q != NULL ; )
1007             if (q->head != NULL || q->S != q->F+1) {
1008                 prev = q ;
1009                 q = q->next ;
1010             } else { /* entry is idle, expire it */
1011                 struct dn_flow_queue *old_q = q ;
1012
1013                 if (prev != NULL)
1014                     prev->next = q = q->next ;
1015                 else
1016                     fs->rq[i] = q = q->next ;
1017                 fs->rq_elements-- ;
1018                 free(old_q, M_DUMMYNET);
1019             }
1020     return initial_elements - fs->rq_elements ;
1021 }
1022
1023 /*
1024  * If room, create a new queue and put at head of slot i;
1025  * otherwise, create or use the default queue.
1026  */
1027 static struct dn_flow_queue *
1028 create_queue(struct dn_flow_set *fs, int i)
1029 {
1030         struct dn_flow_queue *q;
1031
1032         if (fs->rq_elements > fs->rq_size * dn_max_ratio &&
1033             expire_queues(fs) == 0) {
1034                 /* No way to get room, use or create overflow queue. */
1035                 i = fs->rq_size;
1036                 if (fs->rq[i] != NULL)
1037                     return fs->rq[i];
1038         }
1039         q = malloc(sizeof(*q), M_DUMMYNET, M_NOWAIT | M_ZERO);
1040         if (q == NULL) {
1041                 printf("dummynet: sorry, cannot allocate queue for new flow\n");
1042                 return (NULL);
1043         }
1044         q->fs = fs;
1045         q->hash_slot = i;
1046         q->next = fs->rq[i];
1047         q->S = q->F + 1;        /* hack - mark timestamp as invalid. */
1048         q->numbytes = fs->pipe->burst + (io_fast ? fs->pipe->bandwidth : 0);
1049         fs->rq[i] = q;
1050         fs->rq_elements++;
1051         return (q);
1052 }
1053
1054 /*
1055  * Given a flow_set and a pkt in last_pkt, find a matching queue
1056  * after appropriate masking. The queue is moved to front
1057  * so that further searches take less time.
1058  */
1059 static struct dn_flow_queue *
1060 find_queue(struct dn_flow_set *fs, struct ipfw_flow_id *id)
1061 {
1062     int i = 0 ; /* we need i and q for new allocations */
1063     struct dn_flow_queue *q, *prev;
1064     int is_v6 = IS_IP6_FLOW_ID(id);
1065
1066     if ( !(fs->flags_fs & DN_HAVE_FLOW_MASK) )
1067         q = fs->rq[0] ;
1068     else {
1069         /* first, do the masking, then hash */
1070         id->dst_port &= fs->flow_mask.dst_port ;
1071         id->src_port &= fs->flow_mask.src_port ;
1072         id->proto &= fs->flow_mask.proto ;
1073         id->flags = 0 ; /* we don't care about this one */
1074         if (is_v6) {
1075             APPLY_MASK(&id->dst_ip6, &fs->flow_mask.dst_ip6);
1076             APPLY_MASK(&id->src_ip6, &fs->flow_mask.src_ip6);
1077             id->flow_id6 &= fs->flow_mask.flow_id6;
1078
1079             i = ((id->dst_ip6.__u6_addr.__u6_addr32[0]) & 0xffff)^
1080                 ((id->dst_ip6.__u6_addr.__u6_addr32[1]) & 0xffff)^
1081                 ((id->dst_ip6.__u6_addr.__u6_addr32[2]) & 0xffff)^
1082                 ((id->dst_ip6.__u6_addr.__u6_addr32[3]) & 0xffff)^
1083
1084                 ((id->dst_ip6.__u6_addr.__u6_addr32[0] >> 15) & 0xffff)^
1085                 ((id->dst_ip6.__u6_addr.__u6_addr32[1] >> 15) & 0xffff)^
1086                 ((id->dst_ip6.__u6_addr.__u6_addr32[2] >> 15) & 0xffff)^
1087                 ((id->dst_ip6.__u6_addr.__u6_addr32[3] >> 15) & 0xffff)^
1088
1089                 ((id->src_ip6.__u6_addr.__u6_addr32[0] << 1) & 0xfffff)^
1090                 ((id->src_ip6.__u6_addr.__u6_addr32[1] << 1) & 0xfffff)^
1091                 ((id->src_ip6.__u6_addr.__u6_addr32[2] << 1) & 0xfffff)^
1092                 ((id->src_ip6.__u6_addr.__u6_addr32[3] << 1) & 0xfffff)^
1093
1094                 ((id->src_ip6.__u6_addr.__u6_addr32[0] << 16) & 0xffff)^
1095                 ((id->src_ip6.__u6_addr.__u6_addr32[1] << 16) & 0xffff)^
1096                 ((id->src_ip6.__u6_addr.__u6_addr32[2] << 16) & 0xffff)^
1097                 ((id->src_ip6.__u6_addr.__u6_addr32[3] << 16) & 0xffff)^
1098
1099                 (id->dst_port << 1) ^ (id->src_port) ^
1100                 (id->proto ) ^
1101                 (id->flow_id6);
1102         } else {
1103             id->dst_ip &= fs->flow_mask.dst_ip ;
1104             id->src_ip &= fs->flow_mask.src_ip ;
1105
1106             i = ( (id->dst_ip) & 0xffff ) ^
1107                 ( (id->dst_ip >> 15) & 0xffff ) ^
1108                 ( (id->src_ip << 1) & 0xffff ) ^
1109                 ( (id->src_ip >> 16 ) & 0xffff ) ^
1110                 (id->dst_port << 1) ^ (id->src_port) ^
1111                 (id->proto );
1112         }
1113         i = i % fs->rq_size ;
1114         /* finally, scan the current list for a match */
1115         searches++ ;
1116         for (prev=NULL, q = fs->rq[i] ; q ; ) {
1117             search_steps++;
1118             if (is_v6 &&
1119                     IN6_ARE_ADDR_EQUAL(&id->dst_ip6,&q->id.dst_ip6) &&
1120                     IN6_ARE_ADDR_EQUAL(&id->src_ip6,&q->id.src_ip6) &&
1121                     id->dst_port == q->id.dst_port &&
1122                     id->src_port == q->id.src_port &&
1123                     id->proto == q->id.proto &&
1124                     id->flags == q->id.flags &&
1125                     id->flow_id6 == q->id.flow_id6)
1126                 break ; /* found */
1127
1128             if (!is_v6 && id->dst_ip == q->id.dst_ip &&
1129                     id->src_ip == q->id.src_ip &&
1130                     id->dst_port == q->id.dst_port &&
1131                     id->src_port == q->id.src_port &&
1132                     id->proto == q->id.proto &&
1133                     id->flags == q->id.flags)
1134                 break ; /* found */
1135
1136             /* No match. Check if we can expire the entry */
1137             if (pipe_expire && q->head == NULL && q->S == q->F+1 ) {
1138                 /* entry is idle and not in any heap, expire it */
1139                 struct dn_flow_queue *old_q = q ;
1140
1141                 if (prev != NULL)
1142                     prev->next = q = q->next ;
1143                 else
1144                     fs->rq[i] = q = q->next ;
1145                 fs->rq_elements-- ;
1146                 free(old_q, M_DUMMYNET);
1147                 continue ;
1148             }
1149             prev = q ;
1150             q = q->next ;
1151         }
1152         if (q && prev != NULL) { /* found and not in front */
1153             prev->next = q->next ;
1154             q->next = fs->rq[i] ;
1155             fs->rq[i] = q ;
1156         }
1157     }
1158     if (q == NULL) { /* no match, need to allocate a new entry */
1159         q = create_queue(fs, i);
1160         if (q != NULL)
1161         q->id = *id ;
1162     }
1163     return q ;
1164 }
1165
1166 static int
1167 red_drops(struct dn_flow_set *fs, struct dn_flow_queue *q, int len)
1168 {
1169         /*
1170          * RED algorithm
1171          *
1172          * RED calculates the average queue size (avg) using a low-pass filter
1173          * with an exponential weighted (w_q) moving average:
1174          *      avg  <-  (1-w_q) * avg + w_q * q_size
1175          * where q_size is the queue length (measured in bytes or * packets).
1176          *
1177          * If q_size == 0, we compute the idle time for the link, and set
1178          *      avg = (1 - w_q)^(idle/s)
1179          * where s is the time needed for transmitting a medium-sized packet.
1180          *
1181          * Now, if avg < min_th the packet is enqueued.
1182          * If avg > max_th the packet is dropped. Otherwise, the packet is
1183          * dropped with probability P function of avg.
1184          */
1185
1186         int64_t p_b = 0;
1187
1188         /* Queue in bytes or packets? */
1189         u_int q_size = (fs->flags_fs & DN_QSIZE_IS_BYTES) ?
1190             q->len_bytes : q->len;
1191
1192         DPRINTF(("\ndummynet: %d q: %2u ", (int)curr_time, q_size));
1193
1194         /* Average queue size estimation. */
1195         if (q_size != 0) {
1196                 /* Queue is not empty, avg <- avg + (q_size - avg) * w_q */
1197                 int diff = SCALE(q_size) - q->avg;
1198                 int64_t v = SCALE_MUL((int64_t)diff, (int64_t)fs->w_q);
1199
1200                 q->avg += (int)v;
1201         } else {
1202                 /*
1203                  * Queue is empty, find for how long the queue has been
1204                  * empty and use a lookup table for computing
1205                  * (1 - * w_q)^(idle_time/s) where s is the time to send a
1206                  * (small) packet.
1207                  * XXX check wraps...
1208                  */
1209                 if (q->avg) {
1210                         u_int t = (curr_time - q->idle_time) / fs->lookup_step;
1211
1212                         q->avg = (t < fs->lookup_depth) ?
1213                             SCALE_MUL(q->avg, fs->w_q_lookup[t]) : 0;
1214                 }
1215         }
1216         DPRINTF(("dummynet: avg: %u ", SCALE_VAL(q->avg)));
1217
1218         /* Should i drop? */
1219         if (q->avg < fs->min_th) {
1220                 q->count = -1;
1221                 return (0);     /* accept packet */
1222         }
1223         if (q->avg >= fs->max_th) {     /* average queue >=  max threshold */
1224                 if (fs->flags_fs & DN_IS_GENTLE_RED) {
1225                         /*
1226                          * According to Gentle-RED, if avg is greater than
1227                          * max_th the packet is dropped with a probability
1228                          *       p_b = c_3 * avg - c_4
1229                          * where c_3 = (1 - max_p) / max_th
1230                          *       c_4 = 1 - 2 * max_p
1231                          */
1232                         p_b = SCALE_MUL((int64_t)fs->c_3, (int64_t)q->avg) -
1233                             fs->c_4;
1234                 } else {
1235                         q->count = -1;
1236                         DPRINTF(("dummynet: - drop"));
1237                         return (1);
1238                 }
1239         } else if (q->avg > fs->min_th) {
1240                 /*
1241                  * We compute p_b using the linear dropping function
1242                  *       p_b = c_1 * avg - c_2
1243                  * where c_1 = max_p / (max_th - min_th)
1244                  *       c_2 = max_p * min_th / (max_th - min_th)
1245                  */
1246                 p_b = SCALE_MUL((int64_t)fs->c_1, (int64_t)q->avg) - fs->c_2;
1247         }
1248
1249         if (fs->flags_fs & DN_QSIZE_IS_BYTES)
1250                 p_b = (p_b * len) / fs->max_pkt_size;
1251         if (++q->count == 0)
1252                 q->random = random() & 0xffff;
1253         else {
1254                 /*
1255                  * q->count counts packets arrived since last drop, so a greater
1256                  * value of q->count means a greater packet drop probability.
1257                  */
1258                 if (SCALE_MUL(p_b, SCALE((int64_t)q->count)) > q->random) {
1259                         q->count = 0;
1260                         DPRINTF(("dummynet: - red drop"));
1261                         /* After a drop we calculate a new random value. */
1262                         q->random = random() & 0xffff;
1263                         return (1);     /* drop */
1264                 }
1265         }
1266         /* End of RED algorithm. */
1267
1268         return (0);     /* accept */
1269 }
1270
1271 static __inline struct dn_flow_set *
1272 locate_flowset(int fs_nr)
1273 {
1274         struct dn_flow_set *fs;
1275
1276         SLIST_FOREACH(fs, &flowsethash[HASH(fs_nr)], next)
1277                 if (fs->fs_nr == fs_nr)
1278                         return (fs);
1279
1280         return (NULL);
1281 }
1282
1283 static __inline struct dn_pipe *
1284 locate_pipe(int pipe_nr)
1285 {
1286         struct dn_pipe *pipe;
1287
1288         SLIST_FOREACH(pipe, &pipehash[HASH(pipe_nr)], next)
1289                 if (pipe->pipe_nr == pipe_nr)
1290                         return (pipe);
1291
1292         return (NULL);
1293 }
1294
1295 /*
1296  * dummynet hook for packets. Below 'pipe' is a pipe or a queue
1297  * depending on whether WF2Q or fixed bw is used.
1298  *
1299  * pipe_nr      pipe or queue the packet is destined for.
1300  * dir          where shall we send the packet after dummynet.
1301  * m            the mbuf with the packet
1302  * ifp          the 'ifp' parameter from the caller.
1303  *              NULL in ip_input, destination interface in ip_output,
1304  * rule         matching rule, in case of multiple passes
1305  */
1306 static int
1307 dummynet_io(struct mbuf **m0, int dir, struct ip_fw_args *fwa)
1308 {
1309         struct mbuf *m = *m0, *head = NULL, *tail = NULL;
1310         struct dn_pkt_tag *pkt;
1311         struct m_tag *mtag;
1312         struct dn_flow_set *fs = NULL;
1313         struct dn_pipe *pipe;
1314         uint64_t len = m->m_pkthdr.len;
1315         struct dn_flow_queue *q = NULL;
1316         int is_pipe;
1317         ipfw_insn *cmd = ACTION_PTR(fwa->rule);
1318
1319         KASSERT(m->m_nextpkt == NULL,
1320             ("dummynet_io: mbuf queue passed to dummynet"));
1321
1322         if (cmd->opcode == O_LOG)
1323                 cmd += F_LEN(cmd);
1324         if (cmd->opcode == O_ALTQ)
1325                 cmd += F_LEN(cmd);
1326         if (cmd->opcode == O_TAG)
1327                 cmd += F_LEN(cmd);
1328         is_pipe = (cmd->opcode == O_PIPE);
1329
1330         DUMMYNET_LOCK();
1331         io_pkt++;
1332         /*
1333          * This is a dummynet rule, so we expect an O_PIPE or O_QUEUE rule.
1334          *
1335          * XXXGL: probably the pipe->fs and fs->pipe logic here
1336          * below can be simplified.
1337          */
1338         if (is_pipe) {
1339                 pipe = locate_pipe(fwa->cookie);
1340                 if (pipe != NULL)
1341                         fs = &(pipe->fs);
1342         } else
1343                 fs = locate_flowset(fwa->cookie);
1344
1345         if (fs == NULL)
1346                 goto dropit;    /* This queue/pipe does not exist! */
1347         pipe = fs->pipe;
1348         if (pipe == NULL) {     /* Must be a queue, try find a matching pipe. */
1349                 pipe = locate_pipe(fs->parent_nr);
1350                 if (pipe != NULL)
1351                         fs->pipe = pipe;
1352                 else {
1353                         printf("dummynet: no pipe %d for queue %d, drop pkt\n",
1354                             fs->parent_nr, fs->fs_nr);
1355                         goto dropit;
1356                 }
1357         }
1358         q = find_queue(fs, &(fwa->f_id));
1359         if (q == NULL)
1360                 goto dropit;            /* Cannot allocate queue. */
1361
1362         /* Update statistics, then check reasons to drop pkt. */
1363         q->tot_bytes += len;
1364         q->tot_pkts++;
1365         if (fs->plr && random() < fs->plr)
1366                 goto dropit;            /* Random pkt drop. */
1367         if (fs->flags_fs & DN_QSIZE_IS_BYTES) {
1368                 if (q->len_bytes > fs->qsize)
1369                         goto dropit;    /* Queue size overflow. */
1370         } else {
1371                 if (q->len >= fs->qsize)
1372                         goto dropit;    /* Queue count overflow. */
1373         }
1374         if (fs->flags_fs & DN_IS_RED && red_drops(fs, q, len))
1375                 goto dropit;
1376
1377         /* XXX expensive to zero, see if we can remove it. */
1378         mtag = m_tag_get(PACKET_TAG_DUMMYNET,
1379             sizeof(struct dn_pkt_tag), M_NOWAIT | M_ZERO);
1380         if (mtag == NULL)
1381                 goto dropit;            /* Cannot allocate packet header. */
1382         m_tag_prepend(m, mtag);         /* Attach to mbuf chain. */
1383
1384         pkt = (struct dn_pkt_tag *)(mtag + 1);
1385         /*
1386          * Ok, i can handle the pkt now...
1387          * Build and enqueue packet + parameters.
1388          */
1389         pkt->rule = fwa->rule;
1390         pkt->rule_id = fwa->rule_id;
1391         pkt->chain_id = fwa->chain_id;
1392         pkt->dn_dir = dir;
1393
1394         pkt->ifp = fwa->oif;
1395
1396         if (q->head == NULL)
1397                 q->head = m;
1398         else
1399                 q->tail->m_nextpkt = m;
1400         q->tail = m;
1401         q->len++;
1402         q->len_bytes += len;
1403
1404         if (q->head != m)               /* Flow was not idle, we are done. */
1405                 goto done;
1406
1407         if (is_pipe) {                  /* Fixed rate queues. */
1408                 if (q->idle_time < curr_time) {
1409                         /* Calculate available burst size. */
1410                         q->numbytes +=
1411                             (curr_time - q->idle_time) * pipe->bandwidth;
1412                         if (q->numbytes > pipe->burst)
1413                                 q->numbytes = pipe->burst;
1414                         if (io_fast)
1415                                 q->numbytes += pipe->bandwidth;
1416                 }
1417         } else {                        /* WF2Q. */
1418                 if (pipe->idle_time < curr_time) {
1419                         /* Calculate available burst size. */
1420                         pipe->numbytes +=
1421                             (curr_time - pipe->idle_time) * pipe->bandwidth;
1422                         if (pipe->numbytes > pipe->burst)
1423                                 pipe->numbytes = pipe->burst;
1424                         if (io_fast)
1425                                 pipe->numbytes += pipe->bandwidth;
1426                 }
1427                 pipe->idle_time = curr_time;
1428         }
1429         /* Necessary for both: fixed rate & WF2Q queues. */
1430         q->idle_time = curr_time;
1431
1432         /*
1433          * If we reach this point the flow was previously idle, so we need
1434          * to schedule it. This involves different actions for fixed-rate or
1435          * WF2Q queues.
1436          */
1437         if (is_pipe) {
1438                 /* Fixed-rate queue: just insert into the ready_heap. */
1439                 dn_key t = 0;
1440
1441                 if (pipe->bandwidth) {
1442                         q->extra_bits = compute_extra_bits(m, pipe);
1443                         t = set_ticks(m, q, pipe);
1444                 }
1445                 q->sched_time = curr_time;
1446                 if (t == 0)             /* Must process it now. */
1447                         ready_event(q, &head, &tail);
1448                 else
1449                         heap_insert(&ready_heap, curr_time + t , q);
1450         } else {
1451                 /*
1452                  * WF2Q. First, compute start time S: if the flow was
1453                  * idle (S = F + 1) set S to the virtual time V for the
1454                  * controlling pipe, and update the sum of weights for the pipe;
1455                  * otherwise, remove flow from idle_heap and set S to max(F,V).
1456                  * Second, compute finish time F = S + len / weight.
1457                  * Third, if pipe was idle, update V = max(S, V).
1458                  * Fourth, count one more backlogged flow.
1459                  */
1460                 if (DN_KEY_GT(q->S, q->F)) { /* Means timestamps are invalid. */
1461                         q->S = pipe->V;
1462                         pipe->sum += fs->weight; /* Add weight of new queue. */
1463                 } else {
1464                         heap_extract(&(pipe->idle_heap), q);
1465                         q->S = MAX64(q->F, pipe->V);
1466                 }
1467                 q->F = q->S + (len << MY_M) / (uint64_t)fs->weight;
1468
1469                 if (pipe->not_eligible_heap.elements == 0 &&
1470                     pipe->scheduler_heap.elements == 0)
1471                         pipe->V = MAX64(q->S, pipe->V);
1472                 fs->backlogged++;
1473                 /*
1474                  * Look at eligibility. A flow is not eligibile if S>V (when
1475                  * this happens, it means that there is some other flow already
1476                  * scheduled for the same pipe, so the scheduler_heap cannot be
1477                  * empty). If the flow is not eligible we just store it in the
1478                  * not_eligible_heap. Otherwise, we store in the scheduler_heap
1479                  * and possibly invoke ready_event_wfq() right now if there is
1480                  * leftover credit.
1481                  * Note that for all flows in scheduler_heap (SCH), S_i <= V,
1482                  * and for all flows in not_eligible_heap (NEH), S_i > V.
1483                  * So when we need to compute max(V, min(S_i)) forall i in
1484                  * SCH+NEH, we only need to look into NEH.
1485                  */
1486                 if (DN_KEY_GT(q->S, pipe->V)) {         /* Not eligible. */
1487                         if (pipe->scheduler_heap.elements == 0)
1488                                 printf("dummynet: ++ ouch! not eligible but empty scheduler!\n");
1489                         heap_insert(&(pipe->not_eligible_heap), q->S, q);
1490                 } else {
1491                         heap_insert(&(pipe->scheduler_heap), q->F, q);
1492                         if (pipe->numbytes >= 0) {       /* Pipe is idle. */
1493                                 if (pipe->scheduler_heap.elements != 1)
1494                                         printf("dummynet: OUCH! pipe should have been idle!\n");
1495                                 DPRINTF(("dummynet: waking up pipe %d at %d\n",
1496                                     pipe->pipe_nr, (int)(q->F >> MY_M)));
1497                                 pipe->sched_time = curr_time;
1498                                 ready_event_wfq(pipe, &head, &tail);
1499                         }
1500                 }
1501         }
1502 done:
1503         if (head == m && dir != DN_TO_IFB_FWD && dir != DN_TO_ETH_DEMUX &&
1504             dir != DN_TO_ETH_OUT) {     /* Fast io. */
1505                 io_pkt_fast++;
1506                 if (m->m_nextpkt != NULL)
1507                         printf("dummynet: fast io: pkt chain detected!\n");
1508                 head = m->m_nextpkt = NULL;
1509         } else
1510                 *m0 = NULL;             /* Normal io. */
1511
1512         DUMMYNET_UNLOCK();
1513         if (head != NULL)
1514                 dummynet_send(head);
1515         return (0);
1516
1517 dropit:
1518         io_pkt_drop++;
1519         if (q)
1520                 q->drops++;
1521         DUMMYNET_UNLOCK();
1522         m_freem(m);
1523         *m0 = NULL;
1524         return ((fs && (fs->flags_fs & DN_NOERROR)) ? 0 : ENOBUFS);
1525 }
1526
1527 /*
1528  * Below, the rt_unref is only needed when (pkt->dn_dir == DN_TO_IP_OUT)
1529  * Doing this would probably save us the initial bzero of dn_pkt
1530  */
1531 #define DN_FREE_PKT(_m) do {                            \
1532         m_freem(_m);                                    \
1533 } while (0)
1534
1535 /*
1536  * Dispose all packets and flow_queues on a flow_set.
1537  * If all=1, also remove red lookup table and other storage,
1538  * including the descriptor itself.
1539  * For the one in dn_pipe MUST also cleanup ready_heap...
1540  */
1541 static void
1542 purge_flow_set(struct dn_flow_set *fs, int all)
1543 {
1544         struct dn_flow_queue *q, *qn;
1545         int i;
1546
1547         DUMMYNET_LOCK_ASSERT();
1548
1549         for (i = 0; i <= fs->rq_size; i++) {
1550                 for (q = fs->rq[i]; q != NULL; q = qn) {
1551                         struct mbuf *m, *mnext;
1552
1553                         mnext = q->head;
1554                         while ((m = mnext) != NULL) {
1555                                 mnext = m->m_nextpkt;
1556                                 DN_FREE_PKT(m);
1557                         }
1558                         qn = q->next;
1559                         free(q, M_DUMMYNET);
1560                 }
1561                 fs->rq[i] = NULL;
1562         }
1563
1564         fs->rq_elements = 0;
1565         if (all) {
1566                 /* RED - free lookup table. */
1567                 if (fs->w_q_lookup != NULL)
1568                         free(fs->w_q_lookup, M_DUMMYNET);
1569                 if (fs->rq != NULL)
1570                         free(fs->rq, M_DUMMYNET);
1571                 /* If this fs is not part of a pipe, free it. */
1572                 if (fs->pipe == NULL || fs != &(fs->pipe->fs))
1573                         free(fs, M_DUMMYNET);
1574         }
1575 }
1576
1577 /*
1578  * Dispose all packets queued on a pipe (not a flow_set).
1579  * Also free all resources associated to a pipe, which is about
1580  * to be deleted.
1581  */
1582 static void
1583 purge_pipe(struct dn_pipe *pipe)
1584 {
1585     struct mbuf *m, *mnext;
1586
1587     purge_flow_set( &(pipe->fs), 1 );
1588
1589     mnext = pipe->head;
1590     while ((m = mnext) != NULL) {
1591         mnext = m->m_nextpkt;
1592         DN_FREE_PKT(m);
1593     }
1594
1595     heap_free( &(pipe->scheduler_heap) );
1596     heap_free( &(pipe->not_eligible_heap) );
1597     heap_free( &(pipe->idle_heap) );
1598 }
1599
1600 /*
1601  * Delete all pipes and heaps returning memory. Must also
1602  * remove references from all ipfw rules to all pipes.
1603  */
1604 static void
1605 dummynet_flush(void)
1606 {
1607         struct dn_pipe *pipe, *pipe1;
1608         struct dn_flow_set *fs, *fs1;
1609         int i;
1610
1611         DUMMYNET_LOCK();
1612         /* Free heaps so we don't have unwanted events. */
1613         heap_free(&ready_heap);
1614         heap_free(&wfq_ready_heap);
1615         heap_free(&extract_heap);
1616
1617         /*
1618          * Now purge all queued pkts and delete all pipes.
1619          *
1620          * XXXGL: can we merge the for(;;) cycles into one or not?
1621          */
1622         for (i = 0; i < HASHSIZE; i++)
1623                 SLIST_FOREACH_SAFE(fs, &flowsethash[i], next, fs1) {
1624                         SLIST_REMOVE(&flowsethash[i], fs, dn_flow_set, next);
1625                         purge_flow_set(fs, 1);
1626                 }
1627         for (i = 0; i < HASHSIZE; i++)
1628                 SLIST_FOREACH_SAFE(pipe, &pipehash[i], next, pipe1) {
1629                         SLIST_REMOVE(&pipehash[i], pipe, dn_pipe, next);
1630                         purge_pipe(pipe);
1631                         free_pipe(pipe);
1632                 }
1633         DUMMYNET_UNLOCK();
1634 }
1635
1636 /*
1637  * setup RED parameters
1638  */
1639 static int
1640 config_red(struct dn_flow_set *p, struct dn_flow_set *x)
1641 {
1642         int i;
1643
1644         x->w_q = p->w_q;
1645         x->min_th = SCALE(p->min_th);
1646         x->max_th = SCALE(p->max_th);
1647         x->max_p = p->max_p;
1648
1649         x->c_1 = p->max_p / (p->max_th - p->min_th);
1650         x->c_2 = SCALE_MUL(x->c_1, SCALE(p->min_th));
1651
1652         if (x->flags_fs & DN_IS_GENTLE_RED) {
1653                 x->c_3 = (SCALE(1) - p->max_p) / p->max_th;
1654                 x->c_4 = SCALE(1) - 2 * p->max_p;
1655         }
1656
1657         /* If the lookup table already exist, free and create it again. */
1658         if (x->w_q_lookup) {
1659                 free(x->w_q_lookup, M_DUMMYNET);
1660                 x->w_q_lookup = NULL;
1661         }
1662         if (red_lookup_depth == 0) {
1663                 printf("\ndummynet: net.inet.ip.dummynet.red_lookup_depth"
1664                     "must be > 0\n");
1665                 free(x, M_DUMMYNET);
1666                 return (EINVAL);
1667         }
1668         x->lookup_depth = red_lookup_depth;
1669         x->w_q_lookup = (u_int *)malloc(x->lookup_depth * sizeof(int),
1670             M_DUMMYNET, M_NOWAIT);
1671         if (x->w_q_lookup == NULL) {
1672                 printf("dummynet: sorry, cannot allocate red lookup table\n");
1673                 free(x, M_DUMMYNET);
1674                 return(ENOSPC);
1675         }
1676
1677         /* Fill the lookup table with (1 - w_q)^x */
1678         x->lookup_step = p->lookup_step;
1679         x->lookup_weight = p->lookup_weight;
1680         x->w_q_lookup[0] = SCALE(1) - x->w_q;
1681
1682         for (i = 1; i < x->lookup_depth; i++)
1683                 x->w_q_lookup[i] =
1684                     SCALE_MUL(x->w_q_lookup[i - 1], x->lookup_weight);
1685
1686         if (red_avg_pkt_size < 1)
1687                 red_avg_pkt_size = 512;
1688         x->avg_pkt_size = red_avg_pkt_size;
1689         if (red_max_pkt_size < 1)
1690                 red_max_pkt_size = 1500;
1691         x->max_pkt_size = red_max_pkt_size;
1692         return (0);
1693 }
1694
1695 static int
1696 alloc_hash(struct dn_flow_set *x, struct dn_flow_set *pfs)
1697 {
1698     if (x->flags_fs & DN_HAVE_FLOW_MASK) {     /* allocate some slots */
1699         int l = pfs->rq_size;
1700
1701         if (l == 0)
1702             l = dn_hash_size;
1703         if (l < 4)
1704             l = 4;
1705         else if (l > DN_MAX_HASH_SIZE)
1706             l = DN_MAX_HASH_SIZE;
1707         x->rq_size = l;
1708     } else                  /* one is enough for null mask */
1709         x->rq_size = 1;
1710     x->rq = malloc((1 + x->rq_size) * sizeof(struct dn_flow_queue *),
1711             M_DUMMYNET, M_NOWAIT | M_ZERO);
1712     if (x->rq == NULL) {
1713         printf("dummynet: sorry, cannot allocate queue\n");
1714         return (ENOMEM);
1715     }
1716     x->rq_elements = 0;
1717     return 0 ;
1718 }
1719
1720 static void
1721 set_fs_parms(struct dn_flow_set *x, struct dn_flow_set *src)
1722 {
1723         x->flags_fs = src->flags_fs;
1724         x->qsize = src->qsize;
1725         x->plr = src->plr;
1726         x->flow_mask = src->flow_mask;
1727         if (x->flags_fs & DN_QSIZE_IS_BYTES) {
1728                 if (x->qsize > pipe_byte_limit)
1729                         x->qsize = 1024 * 1024;
1730         } else {
1731                 if (x->qsize == 0)
1732                         x->qsize = 50;
1733                 if (x->qsize > pipe_slot_limit)
1734                         x->qsize = 50;
1735         }
1736         /* Configuring RED. */
1737         if (x->flags_fs & DN_IS_RED)
1738                 config_red(src, x);     /* XXX should check errors */
1739 }
1740
1741 /*
1742  * Setup pipe or queue parameters.
1743  */
1744 static int
1745 config_pipe(struct dn_pipe *p)
1746 {
1747         struct dn_flow_set *pfs = &(p->fs);
1748         struct dn_flow_queue *q;
1749         int i, error;
1750
1751         /*
1752          * The config program passes parameters as follows:
1753          * bw = bits/second (0 means no limits),
1754          * delay = ms, must be translated into ticks.
1755          * qsize = slots/bytes
1756          */
1757         p->delay = (p->delay * hz) / 1000;
1758         /* Scale burst size: bytes -> bits * hz */
1759         p->burst *= 8 * hz;
1760         /* We need either a pipe number or a flow_set number. */
1761         if (p->pipe_nr == 0 && pfs->fs_nr == 0)
1762                 return (EINVAL);
1763         if (p->pipe_nr != 0 && pfs->fs_nr != 0)
1764                 return (EINVAL);
1765         if (p->pipe_nr != 0) {                  /* this is a pipe */
1766                 struct dn_pipe *pipe;
1767
1768                 DUMMYNET_LOCK();
1769                 pipe = locate_pipe(p->pipe_nr); /* locate pipe */
1770
1771                 if (pipe == NULL) {             /* new pipe */
1772                         pipe = malloc(sizeof(struct dn_pipe), M_DUMMYNET,
1773                             M_NOWAIT | M_ZERO);
1774                         if (pipe == NULL) {
1775                                 DUMMYNET_UNLOCK();
1776                                 printf("dummynet: no memory for new pipe\n");
1777                                 return (ENOMEM);
1778                         }
1779                         pipe->pipe_nr = p->pipe_nr;
1780                         pipe->fs.pipe = pipe;
1781                         /*
1782                          * idle_heap is the only one from which
1783                          * we extract from the middle.
1784                          */
1785                         pipe->idle_heap.size = pipe->idle_heap.elements = 0;
1786                         pipe->idle_heap.offset =
1787                             offsetof(struct dn_flow_queue, heap_pos);
1788                 } else
1789                         /* Flush accumulated credit for all queues. */
1790                         for (i = 0; i <= pipe->fs.rq_size; i++)
1791                                 for (q = pipe->fs.rq[i]; q; q = q->next) {
1792                                         q->numbytes = p->burst +
1793                                             (io_fast ? p->bandwidth : 0);
1794                                 }
1795
1796                 pipe->bandwidth = p->bandwidth;
1797                 pipe->burst = p->burst;
1798                 pipe->numbytes = pipe->burst + (io_fast ? pipe->bandwidth : 0);
1799                 bcopy(p->if_name, pipe->if_name, sizeof(p->if_name));
1800                 pipe->ifp = NULL;               /* reset interface ptr */
1801                 pipe->delay = p->delay;
1802                 set_fs_parms(&(pipe->fs), pfs);
1803
1804                 /* Handle changes in the delay profile. */
1805                 if (p->samples_no > 0) {
1806                         if (pipe->samples_no != p->samples_no) {
1807                                 if (pipe->samples != NULL)
1808                                         free(pipe->samples, M_DUMMYNET);
1809                                 pipe->samples =
1810                                     malloc(p->samples_no*sizeof(dn_key),
1811                                         M_DUMMYNET, M_NOWAIT | M_ZERO);
1812                                 if (pipe->samples == NULL) {
1813                                         DUMMYNET_UNLOCK();
1814                                         printf("dummynet: no memory "
1815                                                 "for new samples\n");
1816                                         return (ENOMEM);
1817                                 }
1818                                 pipe->samples_no = p->samples_no;
1819                         }
1820
1821                         strncpy(pipe->name,p->name,sizeof(pipe->name));
1822                         pipe->loss_level = p->loss_level;
1823                         for (i = 0; i<pipe->samples_no; ++i)
1824                                 pipe->samples[i] = p->samples[i];
1825                 } else if (pipe->samples != NULL) {
1826                         free(pipe->samples, M_DUMMYNET);
1827                         pipe->samples = NULL;
1828                         pipe->samples_no = 0;
1829                 }
1830
1831                 if (pipe->fs.rq == NULL) {      /* a new pipe */
1832                         error = alloc_hash(&(pipe->fs), pfs);
1833                         if (error) {
1834                                 DUMMYNET_UNLOCK();
1835                                 free_pipe(pipe);
1836                                 return (error);
1837                         }
1838                         SLIST_INSERT_HEAD(&pipehash[HASH(pipe->pipe_nr)],
1839                             pipe, next);
1840                 }
1841                 DUMMYNET_UNLOCK();
1842         } else {                                /* config queue */
1843                 struct dn_flow_set *fs;
1844
1845                 DUMMYNET_LOCK();
1846                 fs = locate_flowset(pfs->fs_nr); /* locate flow_set */
1847
1848                 if (fs == NULL) {               /* new */
1849                         if (pfs->parent_nr == 0) { /* need link to a pipe */
1850                                 DUMMYNET_UNLOCK();
1851                                 return (EINVAL);
1852                         }
1853                         fs = malloc(sizeof(struct dn_flow_set), M_DUMMYNET,
1854                             M_NOWAIT | M_ZERO);
1855                         if (fs == NULL) {
1856                                 DUMMYNET_UNLOCK();
1857                                 printf(
1858                                     "dummynet: no memory for new flow_set\n");
1859                                 return (ENOMEM);
1860                         }
1861                         fs->fs_nr = pfs->fs_nr;
1862                         fs->parent_nr = pfs->parent_nr;
1863                         fs->weight = pfs->weight;
1864                         if (fs->weight == 0)
1865                                 fs->weight = 1;
1866                         else if (fs->weight > 100)
1867                                 fs->weight = 100;
1868                 } else {
1869                         /*
1870                          * Change parent pipe not allowed;
1871                          * must delete and recreate.
1872                          */
1873                         if (pfs->parent_nr != 0 &&
1874                             fs->parent_nr != pfs->parent_nr) {
1875                                 DUMMYNET_UNLOCK();
1876                                 return (EINVAL);
1877                         }
1878                 }
1879
1880                 set_fs_parms(fs, pfs);
1881
1882                 if (fs->rq == NULL) {           /* a new flow_set */
1883                         error = alloc_hash(fs, pfs);
1884                         if (error) {
1885                                 DUMMYNET_UNLOCK();
1886                                 free(fs, M_DUMMYNET);
1887                                 return (error);
1888                         }
1889                         SLIST_INSERT_HEAD(&flowsethash[HASH(fs->fs_nr)],
1890                             fs, next);
1891                 }
1892                 DUMMYNET_UNLOCK();
1893         }
1894         return (0);
1895 }
1896
1897 /*
1898  * Helper function to remove from a heap queues which are linked to
1899  * a flow_set about to be deleted.
1900  */
1901 static void
1902 fs_remove_from_heap(struct dn_heap *h, struct dn_flow_set *fs)
1903 {
1904     int i = 0, found = 0 ;
1905     for (; i < h->elements ;)
1906         if ( ((struct dn_flow_queue *)h->p[i].object)->fs == fs) {
1907             h->elements-- ;
1908             h->p[i] = h->p[h->elements] ;
1909             found++ ;
1910         } else
1911             i++ ;
1912     if (found)
1913         heapify(h);
1914 }
1915
1916 /*
1917  * helper function to remove a pipe from a heap (can be there at most once)
1918  */
1919 static void
1920 pipe_remove_from_heap(struct dn_heap *h, struct dn_pipe *p)
1921 {
1922     if (h->elements > 0) {
1923         int i = 0 ;
1924         for (i=0; i < h->elements ; i++ ) {
1925             if (h->p[i].object == p) { /* found it */
1926                 h->elements-- ;
1927                 h->p[i] = h->p[h->elements] ;
1928                 heapify(h);
1929                 break ;
1930             }
1931         }
1932     }
1933 }
1934
1935 /*
1936  * drain all queues. Called in case of severe mbuf shortage.
1937  */
1938 void
1939 dummynet_drain(void)
1940 {
1941     struct dn_flow_set *fs;
1942     struct dn_pipe *pipe;
1943     struct mbuf *m, *mnext;
1944     int i;
1945
1946     DUMMYNET_LOCK_ASSERT();
1947
1948     heap_free(&ready_heap);
1949     heap_free(&wfq_ready_heap);
1950     heap_free(&extract_heap);
1951     /* remove all references to this pipe from flow_sets */
1952     for (i = 0; i < HASHSIZE; i++)
1953         SLIST_FOREACH(fs, &flowsethash[i], next)
1954                 purge_flow_set(fs, 0);
1955
1956     for (i = 0; i < HASHSIZE; i++) {
1957         SLIST_FOREACH(pipe, &pipehash[i], next) {
1958                 purge_flow_set(&(pipe->fs), 0);
1959
1960                 mnext = pipe->head;
1961                 while ((m = mnext) != NULL) {
1962                         mnext = m->m_nextpkt;
1963                         DN_FREE_PKT(m);
1964                 }
1965                 pipe->head = pipe->tail = NULL;
1966         }
1967     }
1968 }
1969
1970 /*
1971  * Fully delete a pipe or a queue, cleaning up associated info.
1972  */
1973 static int
1974 delete_pipe(struct dn_pipe *p)
1975 {
1976
1977     if (p->pipe_nr == 0 && p->fs.fs_nr == 0)
1978         return EINVAL ;
1979     if (p->pipe_nr != 0 && p->fs.fs_nr != 0)
1980         return EINVAL ;
1981     if (p->pipe_nr != 0) { /* this is an old-style pipe */
1982         struct dn_pipe *pipe;
1983         struct dn_flow_set *fs;
1984         int i;
1985
1986         DUMMYNET_LOCK();
1987         pipe = locate_pipe(p->pipe_nr); /* locate pipe */
1988
1989         if (pipe == NULL) {
1990             DUMMYNET_UNLOCK();
1991             return (ENOENT);    /* not found */
1992         }
1993
1994         /* Unlink from list of pipes. */
1995         SLIST_REMOVE(&pipehash[HASH(pipe->pipe_nr)], pipe, dn_pipe, next);
1996
1997         /* Remove all references to this pipe from flow_sets. */
1998         for (i = 0; i < HASHSIZE; i++)
1999             SLIST_FOREACH(fs, &flowsethash[i], next)
2000                 if (fs->pipe == pipe) {
2001                         printf("dummynet: ++ ref to pipe %d from fs %d\n",
2002                             p->pipe_nr, fs->fs_nr);
2003                         fs->pipe = NULL ;
2004                         purge_flow_set(fs, 0);
2005                 }
2006         fs_remove_from_heap(&ready_heap, &(pipe->fs));
2007         purge_pipe(pipe); /* remove all data associated to this pipe */
2008         /* remove reference to here from extract_heap and wfq_ready_heap */
2009         pipe_remove_from_heap(&extract_heap, pipe);
2010         pipe_remove_from_heap(&wfq_ready_heap, pipe);
2011         DUMMYNET_UNLOCK();
2012
2013         free_pipe(pipe);
2014     } else { /* this is a WF2Q queue (dn_flow_set) */
2015         struct dn_flow_set *fs;
2016
2017         DUMMYNET_LOCK();
2018         fs = locate_flowset(p->fs.fs_nr); /* locate set */
2019
2020         if (fs == NULL) {
2021             DUMMYNET_UNLOCK();
2022             return (ENOENT); /* not found */
2023         }
2024
2025         /* Unlink from list of flowsets. */
2026         SLIST_REMOVE( &flowsethash[HASH(fs->fs_nr)], fs, dn_flow_set, next);
2027
2028         if (fs->pipe != NULL) {
2029             /* Update total weight on parent pipe and cleanup parent heaps. */
2030             fs->pipe->sum -= fs->weight * fs->backlogged ;
2031             fs_remove_from_heap(&(fs->pipe->not_eligible_heap), fs);
2032             fs_remove_from_heap(&(fs->pipe->scheduler_heap), fs);
2033 #if 1   /* XXX should i remove from idle_heap as well ? */
2034             fs_remove_from_heap(&(fs->pipe->idle_heap), fs);
2035 #endif
2036         }
2037         purge_flow_set(fs, 1);
2038         DUMMYNET_UNLOCK();
2039     }
2040     return 0 ;
2041 }
2042
2043 /*
2044  * helper function used to copy data from kernel in DUMMYNET_GET
2045  */
2046 static char *
2047 dn_copy_set(struct dn_flow_set *set, char *bp)
2048 {
2049     int i, copied = 0 ;
2050     struct dn_flow_queue *q, *qp = (struct dn_flow_queue *)bp;
2051
2052     DUMMYNET_LOCK_ASSERT();
2053
2054     for (i = 0 ; i <= set->rq_size ; i++)
2055         for (q = set->rq[i] ; q ; q = q->next, qp++ ) {
2056             if (q->hash_slot != i)
2057                 printf("dummynet: ++ at %d: wrong slot (have %d, "
2058                     "should be %d)\n", copied, q->hash_slot, i);
2059             if (q->fs != set)
2060                 printf("dummynet: ++ at %d: wrong fs ptr (have %p, should be %p)\n",
2061                         i, q->fs, set);
2062             copied++ ;
2063             bcopy(q, qp, sizeof( *q ) );
2064             /* cleanup pointers */
2065             qp->next = NULL ;
2066             qp->head = qp->tail = NULL ;
2067             qp->fs = NULL ;
2068         }
2069     if (copied != set->rq_elements)
2070         printf("dummynet: ++ wrong count, have %d should be %d\n",
2071             copied, set->rq_elements);
2072     return (char *)qp ;
2073 }
2074
2075 static size_t
2076 dn_calc_size(void)
2077 {
2078     struct dn_flow_set *fs;
2079     struct dn_pipe *pipe;
2080     size_t size = 0;
2081     int i;
2082
2083     DUMMYNET_LOCK_ASSERT();
2084     /*
2085      * Compute size of data structures: list of pipes and flow_sets.
2086      */
2087     for (i = 0; i < HASHSIZE; i++) {
2088         SLIST_FOREACH(pipe, &pipehash[i], next)
2089                 size += sizeof(*pipe) +
2090                     pipe->fs.rq_elements * sizeof(struct dn_flow_queue);
2091         SLIST_FOREACH(fs, &flowsethash[i], next)
2092                 size += sizeof (*fs) +
2093                     fs->rq_elements * sizeof(struct dn_flow_queue);
2094     }
2095     return size;
2096 }
2097
2098 static int
2099 dummynet_get(struct sockopt *sopt)
2100 {
2101     char *buf, *bp ; /* bp is the "copy-pointer" */
2102     size_t size ;
2103     struct dn_flow_set *fs;
2104     struct dn_pipe *pipe;
2105     int error=0, i ;
2106
2107     /* XXX lock held too long */
2108     DUMMYNET_LOCK();
2109     /*
2110      * XXX: Ugly, but we need to allocate memory with M_WAITOK flag and we
2111      *      cannot use this flag while holding a mutex.
2112      */
2113     for (i = 0; i < 10; i++) {
2114         size = dn_calc_size();
2115         DUMMYNET_UNLOCK();
2116         buf = malloc(size, M_TEMP, M_WAITOK);
2117         DUMMYNET_LOCK();
2118         if (size == dn_calc_size())
2119                 break;
2120         free(buf, M_TEMP);
2121         buf = NULL;
2122     }
2123     if (buf == NULL) {
2124         DUMMYNET_UNLOCK();
2125         return ENOBUFS ;
2126     }
2127     bp = buf;
2128     for (i = 0; i < HASHSIZE; i++)
2129         SLIST_FOREACH(pipe, &pipehash[i], next) {
2130                 struct dn_pipe *pipe_bp = (struct dn_pipe *)bp;
2131
2132                 /*
2133                  * Copy pipe descriptor into *bp, convert delay back to ms,
2134                  * then copy the flow_set descriptor(s) one at a time.
2135                  * After each flow_set, copy the queue descriptor it owns.
2136                  */
2137                 bcopy(pipe, bp, sizeof(*pipe));
2138                 pipe_bp->delay = (pipe_bp->delay * 1000) / hz;
2139                 pipe_bp->burst /= 8 * hz;
2140                 /*
2141                  * XXX the following is a hack based on ->next being the
2142                  * first field in dn_pipe and dn_flow_set. The correct
2143                  * solution would be to move the dn_flow_set to the beginning
2144                  * of struct dn_pipe.
2145                  */
2146                 pipe_bp->next.sle_next = (struct dn_pipe *)DN_IS_PIPE;
2147                 /* Clean pointers. */
2148                 pipe_bp->head = pipe_bp->tail = NULL;
2149                 pipe_bp->fs.next.sle_next = NULL;
2150                 pipe_bp->fs.pipe = NULL;
2151                 pipe_bp->fs.rq = NULL;
2152                 pipe_bp->samples = NULL;
2153
2154                 bp += sizeof(*pipe) ;
2155                 bp = dn_copy_set(&(pipe->fs), bp);
2156         }
2157
2158     for (i = 0; i < HASHSIZE; i++)
2159         SLIST_FOREACH(fs, &flowsethash[i], next) {
2160                 struct dn_flow_set *fs_bp = (struct dn_flow_set *)bp;
2161
2162                 bcopy(fs, bp, sizeof(*fs));
2163                 /* XXX same hack as above */
2164                 fs_bp->next.sle_next = (struct dn_flow_set *)DN_IS_QUEUE;
2165                 fs_bp->pipe = NULL;
2166                 fs_bp->rq = NULL;
2167                 bp += sizeof(*fs);
2168                 bp = dn_copy_set(fs, bp);
2169         }
2170
2171     DUMMYNET_UNLOCK();
2172
2173     error = sooptcopyout(sopt, buf, size);
2174     free(buf, M_TEMP);
2175     return error ;
2176 }
2177
2178 /*
2179  * Handler for the various dummynet socket options (get, flush, config, del)
2180  */
2181 static int
2182 ip_dn_ctl(struct sockopt *sopt)
2183 {
2184     int error;
2185     struct dn_pipe *p = NULL;
2186
2187     error = priv_check(sopt->sopt_td, PRIV_NETINET_DUMMYNET);
2188     if (error)
2189         return (error);
2190
2191     /* Disallow sets in really-really secure mode. */
2192     if (sopt->sopt_dir == SOPT_SET) {
2193 #if __FreeBSD_version >= 500034
2194         error =  securelevel_ge(sopt->sopt_td->td_ucred, 3);
2195         if (error)
2196             return (error);
2197 #else
2198         if (securelevel >= 3)
2199             return (EPERM);
2200 #endif
2201     }
2202
2203     switch (sopt->sopt_name) {
2204     default :
2205         printf("dummynet: -- unknown option %d", sopt->sopt_name);
2206         error = EINVAL ;
2207         break;
2208
2209     case IP_DUMMYNET_GET :
2210         error = dummynet_get(sopt);
2211         break ;
2212
2213     case IP_DUMMYNET_FLUSH :
2214         dummynet_flush() ;
2215         break ;
2216
2217     case IP_DUMMYNET_CONFIGURE :
2218         p = malloc(sizeof(struct dn_pipe_max), M_TEMP, M_WAITOK);
2219         error = sooptcopyin(sopt, p, sizeof(struct dn_pipe_max), sizeof *p);
2220         if (error)
2221             break ;
2222         if (p->samples_no > 0)
2223             p->samples = &(((struct dn_pipe_max *)p)->samples[0]);
2224
2225         error = config_pipe(p);
2226         break ;
2227
2228     case IP_DUMMYNET_DEL :      /* remove a pipe or queue */
2229         p = malloc(sizeof(struct dn_pipe), M_TEMP, M_WAITOK);
2230         error = sooptcopyin(sopt, p, sizeof(struct dn_pipe), sizeof *p);
2231         if (error)
2232             break ;
2233
2234         error = delete_pipe(p);
2235         break ;
2236     }
2237     if (p != NULL)
2238         free(p, M_TEMP);
2239     return error ;
2240 }
2241
2242 static void
2243 ip_dn_init(void)
2244 {
2245         int i;
2246
2247         if (bootverbose)
2248                 printf("DUMMYNET with IPv6 initialized (040826)\n");
2249
2250         DUMMYNET_LOCK_INIT();
2251
2252         for (i = 0; i < HASHSIZE; i++) {
2253                 SLIST_INIT(&pipehash[i]);
2254                 SLIST_INIT(&flowsethash[i]);
2255         }
2256         ready_heap.size = ready_heap.elements = 0;
2257         ready_heap.offset = 0;
2258
2259         wfq_ready_heap.size = wfq_ready_heap.elements = 0;
2260         wfq_ready_heap.offset = 0;
2261
2262         extract_heap.size = extract_heap.elements = 0;
2263         extract_heap.offset = 0;
2264
2265         ip_dn_ctl_ptr = ip_dn_ctl;
2266         ip_dn_io_ptr = dummynet_io;
2267
2268         TASK_INIT(&dn_task, 0, dummynet_task, NULL);
2269         dn_tq = taskqueue_create_fast("dummynet", M_NOWAIT,
2270             taskqueue_thread_enqueue, &dn_tq);
2271         taskqueue_start_threads(&dn_tq, 1, PI_NET, "dummynet");
2272
2273         callout_init(&dn_timeout, CALLOUT_MPSAFE);
2274         callout_reset(&dn_timeout, 1, dummynet, NULL);
2275
2276         /* Initialize curr_time adjustment mechanics. */
2277         getmicrouptime(&prev_t);
2278 }
2279
2280 #ifdef KLD_MODULE
2281 static void
2282 ip_dn_destroy(void)
2283 {
2284         ip_dn_ctl_ptr = NULL;
2285         ip_dn_io_ptr = NULL;
2286
2287         DUMMYNET_LOCK();
2288         callout_stop(&dn_timeout);
2289         DUMMYNET_UNLOCK();
2290         taskqueue_drain(dn_tq, &dn_task);
2291         taskqueue_free(dn_tq);
2292
2293         dummynet_flush();
2294
2295         DUMMYNET_LOCK_DESTROY();
2296 }
2297 #endif /* KLD_MODULE */
2298
2299 static int
2300 dummynet_modevent(module_t mod, int type, void *data)
2301 {
2302
2303         switch (type) {
2304         case MOD_LOAD:
2305                 if (ip_dn_io_ptr) {
2306                     printf("DUMMYNET already loaded\n");
2307                     return EEXIST ;
2308                 }
2309                 ip_dn_init();
2310                 break;
2311
2312         case MOD_UNLOAD:
2313 #if !defined(KLD_MODULE)
2314                 printf("dummynet statically compiled, cannot unload\n");
2315                 return EINVAL ;
2316 #else
2317                 ip_dn_destroy();
2318 #endif
2319                 break ;
2320         default:
2321                 return EOPNOTSUPP;
2322                 break ;
2323         }
2324         return 0 ;
2325 }
2326
2327 static moduledata_t dummynet_mod = {
2328         "dummynet",
2329         dummynet_modevent,
2330         NULL
2331 };
2332 DECLARE_MODULE(dummynet, dummynet_mod, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY);
2333 MODULE_DEPEND(dummynet, ipfw, 2, 2, 2);
2334 MODULE_VERSION(dummynet, 1);