sys/netpfil/ipfw/ip_dummynet.c

   1 /*-
   2  * Codel/FQ_Codel and PIE/FQ-PIE Code:
   3  * Copyright (C) 2016 Centre for Advanced Internet Architectures,
   4  *  Swinburne University of Technology, Melbourne, Australia.
   5  * Portions of this code were made possible in part by a gift from
   6  *  The Comcast Innovation Fund.
   7  * Implemented by Rasool Al-Saadi <ralsaadi@swin.edu.au>
   8  *
   9  * Copyright (c) 1998-2002,2010 Luigi Rizzo, Universita` di Pisa
  10  * Portions Copyright (c) 2000 Akamba Corp.
  11  * All rights reserved
  12  *
  13  * Redistribution and use in source and binary forms, with or without
  14  * modification, are permitted provided that the following conditions
  15  * are met:
  16  * 1. Redistributions of source code must retain the above copyright
  17  *    notice, this list of conditions and the following disclaimer.
  18  * 2. Redistributions in binary form must reproduce the above copyright
  19  *    notice, this list of conditions and the following disclaimer in the
  20  *    documentation and/or other materials provided with the distribution.
  21  *
  22  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  23  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  25  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  28  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  29  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  31  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  32  * SUCH DAMAGE.
  33  */
  34
  35 #include <sys/cdefs.h>
  36 __FBSDID("$FreeBSD$");
  37
  38 /*
  39  * Configuration and internal object management for dummynet.
  40  */
  41
  42 #include "opt_inet6.h"
  43
  44 #include <sys/param.h>
  45 #include <sys/systm.h>
  46 #include <sys/malloc.h>
  47 #include <sys/mbuf.h>
  48 #include <sys/kernel.h>
  49 #include <sys/lock.h>
  50 #include <sys/module.h>
  51 #include <sys/priv.h>
  52 #include <sys/proc.h>
  53 #include <sys/rwlock.h>
  54 #include <sys/socket.h>
  55 #include <sys/socketvar.h>
  56 #include <sys/time.h>
  57 #include <sys/taskqueue.h>
  58 #include <net/if.h>     /* IFNAMSIZ, struct ifaddr, ifq head, lock.h mutex.h */
  59 #include <netinet/in.h>
  60 #include <netinet/ip_var.h>     /* ip_output(), IP_FORWARDING */
  61 #include <netinet/ip_fw.h>
  62 #include <netinet/ip_dummynet.h>
  63
  64 #include <netpfil/ipfw/ip_fw_private.h>
  65 #include <netpfil/ipfw/dn_heap.h>
  66 #include <netpfil/ipfw/ip_dn_private.h>
  67 #ifdef NEW_AQM
  68 #include <netpfil/ipfw/dn_aqm.h>
  69 #endif
  70 #include <netpfil/ipfw/dn_sched.h>
  71
  72 /* which objects to copy */
  73 #define DN_C_LINK       0x01
  74 #define DN_C_SCH        0x02
  75 #define DN_C_FLOW       0x04
  76 #define DN_C_FS         0x08
  77 #define DN_C_QUEUE      0x10
  78
  79 /* we use this argument in case of a schk_new */
  80 struct schk_new_arg {
  81         struct dn_alg *fp;
  82         struct dn_sch *sch;
  83 };
  84
  85 /*---- callout hooks. ----*/
  86 static struct callout dn_timeout;
  87 static int dn_gone;
  88 static struct task      dn_task;
  89 static struct taskqueue *dn_tq = NULL;
  90
  91 static void
  92 dummynet(void *arg)
  93 {
  94
  95         (void)arg;      /* UNUSED */
  96         taskqueue_enqueue_fast(dn_tq, &dn_task);
  97 }
  98
  99 void
 100 dn_reschedule(void)
 101 {
 102
 103         if (dn_gone != 0)
 104                 return;
 105         callout_reset_sbt(&dn_timeout, tick_sbt, 0, dummynet, NULL,
 106             C_HARDCLOCK | C_DIRECT_EXEC);
 107 }
 108 /*----- end of callout hooks -----*/
 109
 110 #ifdef NEW_AQM
 111 /* Return AQM descriptor for given type or name. */
 112 static struct dn_aqm *
 113 find_aqm_type(int type, char *name)
 114 {
 115         struct dn_aqm *d;
 116
 117         SLIST_FOREACH(d, &dn_cfg.aqmlist, next) {
 118                 if (d->type == type || (name && !strcasecmp(d->name, name)))
 119                         return d;
 120         }
 121         return NULL; /* not found */
 122 }
 123 #endif
 124
 125 /* Return a scheduler descriptor given the type or name. */
 126 static struct dn_alg *
 127 find_sched_type(int type, char *name)
 128 {
 129         struct dn_alg *d;
 130
 131         SLIST_FOREACH(d, &dn_cfg.schedlist, next) {
 132                 if (d->type == type || (name && !strcasecmp(d->name, name)))
 133                         return d;
 134         }
 135         return NULL; /* not found */
 136 }
 137
 138 int
 139 ipdn_bound_var(int *v, int dflt, int lo, int hi, const char *msg)
 140 {
 141         int oldv = *v;
 142         const char *op = NULL;
 143         if (dflt < lo)
 144                 dflt = lo;
 145         if (dflt > hi)
 146                 dflt = hi;
 147         if (oldv < lo) {
 148                 *v = dflt;
 149                 op = "Bump";
 150         } else if (oldv > hi) {
 151                 *v = hi;
 152                 op = "Clamp";
 153         } else
 154                 return *v;
 155         if (op && msg)
 156                 printf("%s %s to %d (was %d)\n", op, msg, *v, oldv);
 157         return *v;
 158 }
 159
 160 /*---- flow_id mask, hash and compare functions ---*/
 161 /*
 162  * The flow_id includes the 5-tuple, the queue/pipe number
 163  * which we store in the extra area in host order,
 164  * and for ipv6 also the flow_id6.
 165  * XXX see if we want the tos byte (can store in 'flags')
 166  */
 167 static struct ipfw_flow_id *
 168 flow_id_mask(struct ipfw_flow_id *mask, struct ipfw_flow_id *id)
 169 {
 170         int is_v6 = IS_IP6_FLOW_ID(id);
 171
 172         id->dst_port &= mask->dst_port;
 173         id->src_port &= mask->src_port;
 174         id->proto &= mask->proto;
 175         id->extra &= mask->extra;
 176         if (is_v6) {
 177                 APPLY_MASK(&id->dst_ip6, &mask->dst_ip6);
 178                 APPLY_MASK(&id->src_ip6, &mask->src_ip6);
 179                 id->flow_id6 &= mask->flow_id6;
 180         } else {
 181                 id->dst_ip &= mask->dst_ip;
 182                 id->src_ip &= mask->src_ip;
 183         }
 184         return id;
 185 }
 186
 187 /* computes an OR of two masks, result in dst and also returned */
 188 static struct ipfw_flow_id *
 189 flow_id_or(struct ipfw_flow_id *src, struct ipfw_flow_id *dst)
 190 {
 191         int is_v6 = IS_IP6_FLOW_ID(dst);
 192
 193         dst->dst_port |= src->dst_port;
 194         dst->src_port |= src->src_port;
 195         dst->proto |= src->proto;
 196         dst->extra |= src->extra;
 197         if (is_v6) {
 198 #define OR_MASK(_d, _s)                          \
 199     (_d)->__u6_addr.__u6_addr32[0] |= (_s)->__u6_addr.__u6_addr32[0]; \
 200     (_d)->__u6_addr.__u6_addr32[1] |= (_s)->__u6_addr.__u6_addr32[1]; \
 201     (_d)->__u6_addr.__u6_addr32[2] |= (_s)->__u6_addr.__u6_addr32[2]; \
 202     (_d)->__u6_addr.__u6_addr32[3] |= (_s)->__u6_addr.__u6_addr32[3];
 203                 OR_MASK(&dst->dst_ip6, &src->dst_ip6);
 204                 OR_MASK(&dst->src_ip6, &src->src_ip6);
 205 #undef OR_MASK
 206                 dst->flow_id6 |= src->flow_id6;
 207         } else {
 208                 dst->dst_ip |= src->dst_ip;
 209                 dst->src_ip |= src->src_ip;
 210         }
 211         return dst;
 212 }
 213
 214 static int
 215 nonzero_mask(struct ipfw_flow_id *m)
 216 {
 217         if (m->dst_port || m->src_port || m->proto || m->extra)
 218                 return 1;
 219         if (IS_IP6_FLOW_ID(m)) {
 220                 return
 221                         m->dst_ip6.__u6_addr.__u6_addr32[0] ||
 222                         m->dst_ip6.__u6_addr.__u6_addr32[1] ||
 223                         m->dst_ip6.__u6_addr.__u6_addr32[2] ||
 224                         m->dst_ip6.__u6_addr.__u6_addr32[3] ||
 225                         m->src_ip6.__u6_addr.__u6_addr32[0] ||
 226                         m->src_ip6.__u6_addr.__u6_addr32[1] ||
 227                         m->src_ip6.__u6_addr.__u6_addr32[2] ||
 228                         m->src_ip6.__u6_addr.__u6_addr32[3] ||
 229                         m->flow_id6;
 230         } else {
 231                 return m->dst_ip || m->src_ip;
 232         }
 233 }
 234
 235 /* XXX we may want a better hash function */
 236 static uint32_t
 237 flow_id_hash(struct ipfw_flow_id *id)
 238 {
 239     uint32_t i;
 240
 241     if (IS_IP6_FLOW_ID(id)) {
 242         uint32_t *d = (uint32_t *)&id->dst_ip6;
 243         uint32_t *s = (uint32_t *)&id->src_ip6;
 244         i = (d[0]      ) ^ (d[1])       ^
 245             (d[2]      ) ^ (d[3])       ^
 246             (d[0] >> 15) ^ (d[1] >> 15) ^
 247             (d[2] >> 15) ^ (d[3] >> 15) ^
 248             (s[0] <<  1) ^ (s[1] <<  1) ^
 249             (s[2] <<  1) ^ (s[3] <<  1) ^
 250             (s[0] << 16) ^ (s[1] << 16) ^
 251             (s[2] << 16) ^ (s[3] << 16) ^
 252             (id->dst_port << 1) ^ (id->src_port) ^
 253             (id->extra) ^
 254             (id->proto ) ^ (id->flow_id6);
 255     } else {
 256         i = (id->dst_ip)        ^ (id->dst_ip >> 15) ^
 257             (id->src_ip << 1)   ^ (id->src_ip >> 16) ^
 258             (id->extra) ^
 259             (id->dst_port << 1) ^ (id->src_port)     ^ (id->proto);
 260     }
 261     return i;
 262 }
 263
 264 /* Like bcmp, returns 0 if ids match, 1 otherwise. */
 265 static int
 266 flow_id_cmp(struct ipfw_flow_id *id1, struct ipfw_flow_id *id2)
 267 {
 268         int is_v6 = IS_IP6_FLOW_ID(id1);
 269
 270         if (!is_v6) {
 271             if (IS_IP6_FLOW_ID(id2))
 272                 return 1; /* different address families */
 273
 274             return (id1->dst_ip == id2->dst_ip &&
 275                     id1->src_ip == id2->src_ip &&
 276                     id1->dst_port == id2->dst_port &&
 277                     id1->src_port == id2->src_port &&
 278                     id1->proto == id2->proto &&
 279                     id1->extra == id2->extra) ? 0 : 1;
 280         }
 281         /* the ipv6 case */
 282         return (
 283             !bcmp(&id1->dst_ip6,&id2->dst_ip6, sizeof(id1->dst_ip6)) &&
 284             !bcmp(&id1->src_ip6,&id2->src_ip6, sizeof(id1->src_ip6)) &&
 285             id1->dst_port == id2->dst_port &&
 286             id1->src_port == id2->src_port &&
 287             id1->proto == id2->proto &&
 288             id1->extra == id2->extra &&
 289             id1->flow_id6 == id2->flow_id6) ? 0 : 1;
 290 }
 291 /*--------- end of flow-id mask, hash and compare ---------*/
 292
 293 /*--- support functions for the qht hashtable ----
 294  * Entries are hashed by flow-id
 295  */
 296 static uint32_t
 297 q_hash(uintptr_t key, int flags, void *arg)
 298 {
 299         /* compute the hash slot from the flow id */
 300         struct ipfw_flow_id *id = (flags & DNHT_KEY_IS_OBJ) ?
 301                 &((struct dn_queue *)key)->ni.fid :
 302                 (struct ipfw_flow_id *)key;
 303
 304         return flow_id_hash(id);
 305 }
 306
 307 static int
 308 q_match(void *obj, uintptr_t key, int flags, void *arg)
 309 {
 310         struct dn_queue *o = (struct dn_queue *)obj;
 311         struct ipfw_flow_id *id2;
 312
 313         if (flags & DNHT_KEY_IS_OBJ) {
 314                 /* compare pointers */
 315                 id2 = &((struct dn_queue *)key)->ni.fid;
 316         } else {
 317                 id2 = (struct ipfw_flow_id *)key;
 318         }
 319         return (0 == flow_id_cmp(&o->ni.fid,  id2));
 320 }
 321
 322 /*
 323  * create a new queue instance for the given 'key'.
 324  */
 325 static void *
 326 q_new(uintptr_t key, int flags, void *arg)
 327 {
 328         struct dn_queue *q, *template = arg;
 329         struct dn_fsk *fs = template->fs;
 330         int size = sizeof(*q) + fs->sched->fp->q_datalen;
 331
 332         q = malloc(size, M_DUMMYNET, M_NOWAIT | M_ZERO);
 333         if (q == NULL) {
 334                 D("no memory for new queue");
 335                 return NULL;
 336         }
 337
 338         set_oid(&q->ni.oid, DN_QUEUE, size);
 339         if (fs->fs.flags & DN_QHT_HASH)
 340                 q->ni.fid = *(struct ipfw_flow_id *)key;
 341         q->fs = fs;
 342         q->_si = template->_si;
 343         q->_si->q_count++;
 344
 345         if (fs->sched->fp->new_queue)
 346                 fs->sched->fp->new_queue(q);
 347
 348 #ifdef NEW_AQM
 349         /* call AQM init function after creating a queue*/
 350         if (fs->aqmfp && fs->aqmfp->init)
 351                 if(fs->aqmfp->init(q))
 352                         D("unable to init AQM for fs %d", fs->fs.fs_nr);
 353 #endif
 354         dn_cfg.queue_count++;
 355
 356         return q;
 357 }
 358
 359 /*
 360  * Notify schedulers that a queue is going away.
 361  * If (flags & DN_DESTROY), also free the packets.
 362  * The version for callbacks is called q_delete_cb().
 363  */
 364 static void
 365 dn_delete_queue(struct dn_queue *q, int flags)
 366 {
 367         struct dn_fsk *fs = q->fs;
 368
 369 #ifdef NEW_AQM
 370         /* clean up AQM status for queue 'q'
 371          * cleanup here is called just with MULTIQUEUE
 372          */
 373         if (fs && fs->aqmfp && fs->aqmfp->cleanup)
 374                 fs->aqmfp->cleanup(q);
 375 #endif
 376         // D("fs %p si %p\n", fs, q->_si);
 377         /* notify the parent scheduler that the queue is going away */
 378         if (fs && fs->sched->fp->free_queue)
 379                 fs->sched->fp->free_queue(q);
 380         q->_si->q_count--;
 381         q->_si = NULL;
 382         if (flags & DN_DESTROY) {
 383                 if (q->mq.head)
 384                         dn_free_pkts(q->mq.head);
 385                 bzero(q, sizeof(*q));   // safety
 386                 free(q, M_DUMMYNET);
 387                 dn_cfg.queue_count--;
 388         }
 389 }
 390
 391 static int
 392 q_delete_cb(void *q, void *arg)
 393 {
 394         int flags = (int)(uintptr_t)arg;
 395         dn_delete_queue(q, flags);
 396         return (flags & DN_DESTROY) ? DNHT_SCAN_DEL : 0;
 397 }
 398
 399 /*
 400  * calls dn_delete_queue/q_delete_cb on all queues,
 401  * which notifies the parent scheduler and possibly drains packets.
 402  * flags & DN_DESTROY: drains queues and destroy qht;
 403  */
 404 static void
 405 qht_delete(struct dn_fsk *fs, int flags)
 406 {
 407         ND("fs %d start flags %d qht %p",
 408                 fs->fs.fs_nr, flags, fs->qht);
 409         if (!fs->qht)
 410                 return;
 411         if (fs->fs.flags & DN_QHT_HASH) {
 412                 dn_ht_scan(fs->qht, q_delete_cb, (void *)(uintptr_t)flags);
 413                 if (flags & DN_DESTROY) {
 414                         dn_ht_free(fs->qht, 0);
 415                         fs->qht = NULL;
 416                 }
 417         } else {
 418                 dn_delete_queue((struct dn_queue *)(fs->qht), flags);
 419                 if (flags & DN_DESTROY)
 420                         fs->qht = NULL;
 421         }
 422 }
 423
 424 /*
 425  * Find and possibly create the queue for a MULTIQUEUE scheduler.
 426  * We never call it for !MULTIQUEUE (the queue is in the sch_inst).
 427  */
 428 struct dn_queue *
 429 ipdn_q_find(struct dn_fsk *fs, struct dn_sch_inst *si,
 430         struct ipfw_flow_id *id)
 431 {
 432         struct dn_queue template;
 433
 434         template._si = si;
 435         template.fs = fs;
 436
 437         if (fs->fs.flags & DN_QHT_HASH) {
 438                 struct ipfw_flow_id masked_id;
 439                 if (fs->qht == NULL) {
 440                         fs->qht = dn_ht_init(NULL, fs->fs.buckets,
 441                                 offsetof(struct dn_queue, q_next),
 442                                 q_hash, q_match, q_new);
 443                         if (fs->qht == NULL)
 444                                 return NULL;
 445                 }
 446                 masked_id = *id;
 447                 flow_id_mask(&fs->fsk_mask, &masked_id);
 448                 return dn_ht_find(fs->qht, (uintptr_t)&masked_id,
 449                         DNHT_INSERT, &template);
 450         } else {
 451                 if (fs->qht == NULL)
 452                         fs->qht = q_new(0, 0, &template);
 453                 return (struct dn_queue *)fs->qht;
 454         }
 455 }
 456 /*--- end of queue hash table ---*/
 457
 458 /*--- support functions for the sch_inst hashtable ----
 459  *
 460  * These are hashed by flow-id
 461  */
 462 static uint32_t
 463 si_hash(uintptr_t key, int flags, void *arg)
 464 {
 465         /* compute the hash slot from the flow id */
 466         struct ipfw_flow_id *id = (flags & DNHT_KEY_IS_OBJ) ?
 467                 &((struct dn_sch_inst *)key)->ni.fid :
 468                 (struct ipfw_flow_id *)key;
 469
 470         return flow_id_hash(id);
 471 }
 472
 473 static int
 474 si_match(void *obj, uintptr_t key, int flags, void *arg)
 475 {
 476         struct dn_sch_inst *o = obj;
 477         struct ipfw_flow_id *id2;
 478
 479         id2 = (flags & DNHT_KEY_IS_OBJ) ?
 480                 &((struct dn_sch_inst *)key)->ni.fid :
 481                 (struct ipfw_flow_id *)key;
 482         return flow_id_cmp(&o->ni.fid,  id2) == 0;
 483 }
 484
 485 /*
 486  * create a new instance for the given 'key'
 487  * Allocate memory for instance, delay line and scheduler private data.
 488  */
 489 static void *
 490 si_new(uintptr_t key, int flags, void *arg)
 491 {
 492         struct dn_schk *s = arg;
 493         struct dn_sch_inst *si;
 494         int l = sizeof(*si) + s->fp->si_datalen;
 495
 496         si = malloc(l, M_DUMMYNET, M_NOWAIT | M_ZERO);
 497         if (si == NULL)
 498                 goto error;
 499
 500         /* Set length only for the part passed up to userland. */
 501         set_oid(&si->ni.oid, DN_SCH_I, sizeof(struct dn_flow));
 502         set_oid(&(si->dline.oid), DN_DELAY_LINE,
 503                 sizeof(struct delay_line));
 504         /* mark si and dline as outside the event queue */
 505         si->ni.oid.id = si->dline.oid.id = -1;
 506
 507         si->sched = s;
 508         si->dline.si = si;
 509
 510         if (s->fp->new_sched && s->fp->new_sched(si)) {
 511                 D("new_sched error");
 512                 goto error;
 513         }
 514         if (s->sch.flags & DN_HAVE_MASK)
 515                 si->ni.fid = *(struct ipfw_flow_id *)key;
 516
 517 #ifdef NEW_AQM
 518         /* init AQM status for !DN_MULTIQUEUE sched*/
 519         if (!(s->fp->flags & DN_MULTIQUEUE))
 520                 if (s->fs->aqmfp && s->fs->aqmfp->init)
 521                         if(s->fs->aqmfp->init((struct dn_queue *)(si + 1))) {
 522                                 D("unable to init AQM for fs %d", s->fs->fs.fs_nr);
 523                                 goto error;
 524                         }
 525 #endif
 526
 527         dn_cfg.si_count++;
 528         return si;
 529
 530 error:
 531         if (si) {
 532                 bzero(si, sizeof(*si)); // safety
 533                 free(si, M_DUMMYNET);
 534         }
 535         return NULL;
 536 }
 537
 538 /*
 539  * Callback from siht to delete all scheduler instances. Remove
 540  * si and delay line from the system heap, destroy all queues.
 541  * We assume that all flowset have been notified and do not
 542  * point to us anymore.
 543  */
 544 static int
 545 si_destroy(void *_si, void *arg)
 546 {
 547         struct dn_sch_inst *si = _si;
 548         struct dn_schk *s = si->sched;
 549         struct delay_line *dl = &si->dline;
 550
 551         if (dl->oid.subtype) /* remove delay line from event heap */
 552                 heap_extract(&dn_cfg.evheap, dl);
 553         dn_free_pkts(dl->mq.head);      /* drain delay line */
 554         if (si->kflags & DN_ACTIVE) /* remove si from event heap */
 555                 heap_extract(&dn_cfg.evheap, si);
 556
 557 #ifdef NEW_AQM
 558         /* clean up AQM status for !DN_MULTIQUEUE sched
 559          * Note that all queues belong to fs were cleaned up in fsk_detach.
 560          * When drain_scheduler is called s->fs and q->fs are pointing
 561          * to a correct fs, so we can use fs in this case.
 562          */
 563         if (!(s->fp->flags & DN_MULTIQUEUE)) {
 564                 struct dn_queue *q = (struct dn_queue *)(si + 1);
 565                 if (q->aqm_status && q->fs->aqmfp)
 566                         if (q->fs->aqmfp->cleanup)
 567                                 q->fs->aqmfp->cleanup(q);
 568         }
 569 #endif
 570         if (s->fp->free_sched)
 571                 s->fp->free_sched(si);
 572         bzero(si, sizeof(*si)); /* safety */
 573         free(si, M_DUMMYNET);
 574         dn_cfg.si_count--;
 575         return DNHT_SCAN_DEL;
 576 }
 577
 578 /*
 579  * Find the scheduler instance for this packet. If we need to apply
 580  * a mask, do on a local copy of the flow_id to preserve the original.
 581  * Assume siht is always initialized if we have a mask.
 582  */
 583 struct dn_sch_inst *
 584 ipdn_si_find(struct dn_schk *s, struct ipfw_flow_id *id)
 585 {
 586
 587         if (s->sch.flags & DN_HAVE_MASK) {
 588                 struct ipfw_flow_id id_t = *id;
 589                 flow_id_mask(&s->sch.sched_mask, &id_t);
 590                 return dn_ht_find(s->siht, (uintptr_t)&id_t,
 591                         DNHT_INSERT, s);
 592         }
 593         if (!s->siht)
 594                 s->siht = si_new(0, 0, s);
 595         return (struct dn_sch_inst *)s->siht;
 596 }
 597
 598 /* callback to flush credit for the scheduler instance */
 599 static int
 600 si_reset_credit(void *_si, void *arg)
 601 {
 602         struct dn_sch_inst *si = _si;
 603         struct dn_link *p = &si->sched->link;
 604
 605         si->credit = p->burst + (dn_cfg.io_fast ?  p->bandwidth : 0);
 606         return 0;
 607 }
 608
 609 static void
 610 schk_reset_credit(struct dn_schk *s)
 611 {
 612         if (s->sch.flags & DN_HAVE_MASK)
 613                 dn_ht_scan(s->siht, si_reset_credit, NULL);
 614         else if (s->siht)
 615                 si_reset_credit(s->siht, NULL);
 616 }
 617 /*---- end of sch_inst hashtable ---------------------*/
 618
 619 /*-------------------------------------------------------
 620  * flowset hash (fshash) support. Entries are hashed by fs_nr.
 621  * New allocations are put in the fsunlinked list, from which
 622  * they are removed when they point to a specific scheduler.
 623  */
 624 static uint32_t
 625 fsk_hash(uintptr_t key, int flags, void *arg)
 626 {
 627         uint32_t i = !(flags & DNHT_KEY_IS_OBJ) ? key :
 628                 ((struct dn_fsk *)key)->fs.fs_nr;
 629
 630         return ( (i>>8)^(i>>4)^i );
 631 }
 632
 633 static int
 634 fsk_match(void *obj, uintptr_t key, int flags, void *arg)
 635 {
 636         struct dn_fsk *fs = obj;
 637         int i = !(flags & DNHT_KEY_IS_OBJ) ? key :
 638                 ((struct dn_fsk *)key)->fs.fs_nr;
 639
 640         return (fs->fs.fs_nr == i);
 641 }
 642
 643 static void *
 644 fsk_new(uintptr_t key, int flags, void *arg)
 645 {
 646         struct dn_fsk *fs;
 647
 648         fs = malloc(sizeof(*fs), M_DUMMYNET, M_NOWAIT | M_ZERO);
 649         if (fs) {
 650                 set_oid(&fs->fs.oid, DN_FS, sizeof(fs->fs));
 651                 dn_cfg.fsk_count++;
 652                 fs->drain_bucket = 0;
 653                 SLIST_INSERT_HEAD(&dn_cfg.fsu, fs, sch_chain);
 654         }
 655         return fs;
 656 }
 657
 658 #ifdef NEW_AQM
 659 /* callback function for cleaning up AQM queue status belongs to a flowset
 660  * connected to scheduler instance '_si' (for !DN_MULTIQUEUE only).
 661  */
 662 static int
 663 si_cleanup_q(void *_si, void *arg)
 664 {
 665         struct dn_sch_inst *si = _si;
 666
 667         if (!(si->sched->fp->flags & DN_MULTIQUEUE)) {
 668                 if (si->sched->fs->aqmfp && si->sched->fs->aqmfp->cleanup)
 669                         si->sched->fs->aqmfp->cleanup((struct dn_queue *) (si+1));
 670         }
 671         return 0;
 672 }
 673
 674 /* callback to clean up queue AQM status.*/
 675 static int
 676 q_cleanup_q(void *_q, void *arg)
 677 {
 678         struct dn_queue *q = _q;
 679         q->fs->aqmfp->cleanup(q);
 680         return 0;
 681 }
 682
 683 /* Clean up all AQM queues status belongs to flowset 'fs' and then
 684  * deconfig AQM for flowset 'fs'
 685  */
 686 static void
 687 aqm_cleanup_deconfig_fs(struct dn_fsk *fs)
 688 {
 689         struct dn_sch_inst *si;
 690
 691         /* clean up AQM status for all queues for !DN_MULTIQUEUE sched*/
 692         if (fs->fs.fs_nr > DN_MAX_ID) {
 693                 if (fs->sched && !(fs->sched->fp->flags & DN_MULTIQUEUE)) {
 694                         if (fs->sched->sch.flags & DN_HAVE_MASK)
 695                                 dn_ht_scan(fs->sched->siht, si_cleanup_q, NULL);
 696                         else {
 697                                         /* single si i.e. no sched mask */
 698                                         si = (struct dn_sch_inst *) fs->sched->siht;
 699                                         if (si && fs->aqmfp && fs->aqmfp->cleanup)
 700                                                 fs->aqmfp->cleanup((struct dn_queue *) (si+1));
 701                         }
 702                 }
 703         }
 704
 705         /* clean up AQM status for all queues for DN_MULTIQUEUE sched*/
 706         if (fs->sched && fs->sched->fp->flags & DN_MULTIQUEUE && fs->qht) {
 707                         if (fs->fs.flags & DN_QHT_HASH)
 708                                 dn_ht_scan(fs->qht, q_cleanup_q, NULL);
 709                         else
 710                                 fs->aqmfp->cleanup((struct dn_queue *)(fs->qht));
 711         }
 712
 713         /* deconfig AQM */
 714         if(fs->aqmcfg && fs->aqmfp && fs->aqmfp->deconfig)
 715                 fs->aqmfp->deconfig(fs);
 716 }
 717 #endif
 718
 719 /*
 720  * detach flowset from its current scheduler. Flags as follows:
 721  * DN_DETACH removes from the fsk_list
 722  * DN_DESTROY deletes individual queues
 723  * DN_DELETE_FS destroys the flowset (otherwise goes in unlinked).
 724  */
 725 static void
 726 fsk_detach(struct dn_fsk *fs, int flags)
 727 {
 728         if (flags & DN_DELETE_FS)
 729                 flags |= DN_DESTROY;
 730         ND("fs %d from sched %d flags %s %s %s",
 731                 fs->fs.fs_nr, fs->fs.sched_nr,
 732                 (flags & DN_DELETE_FS) ? "DEL_FS":"",
 733                 (flags & DN_DESTROY) ? "DEL":"",
 734                 (flags & DN_DETACH) ? "DET":"");
 735         if (flags & DN_DETACH) { /* detach from the list */
 736                 struct dn_fsk_head *h;
 737                 h = fs->sched ? &fs->sched->fsk_list : &dn_cfg.fsu;
 738                 SLIST_REMOVE(h, fs, dn_fsk, sch_chain);
 739         }
 740         /* Free the RED parameters, they will be recomputed on
 741          * subsequent attach if needed.
 742          */
 743         if (fs->w_q_lookup)
 744                 free(fs->w_q_lookup, M_DUMMYNET);
 745         fs->w_q_lookup = NULL;
 746         qht_delete(fs, flags);
 747 #ifdef NEW_AQM
 748         aqm_cleanup_deconfig_fs(fs);
 749 #endif
 750
 751         if (fs->sched && fs->sched->fp->free_fsk)
 752                 fs->sched->fp->free_fsk(fs);
 753         fs->sched = NULL;
 754         if (flags & DN_DELETE_FS) {
 755                 bzero(fs, sizeof(*fs)); /* safety */
 756                 free(fs, M_DUMMYNET);
 757                 dn_cfg.fsk_count--;
 758         } else {
 759                 SLIST_INSERT_HEAD(&dn_cfg.fsu, fs, sch_chain);
 760         }
 761 }
 762
 763 /*
 764  * Detach or destroy all flowsets in a list.
 765  * flags specifies what to do:
 766  * DN_DESTROY:  flush all queues
 767  * DN_DELETE_FS:        DN_DESTROY + destroy flowset
 768  *      DN_DELETE_FS implies DN_DESTROY
 769  */
 770 static void
 771 fsk_detach_list(struct dn_fsk_head *h, int flags)
 772 {
 773         struct dn_fsk *fs;
 774         int n = 0; /* only for stats */
 775
 776         ND("head %p flags %x", h, flags);
 777         while ((fs = SLIST_FIRST(h))) {
 778                 SLIST_REMOVE_HEAD(h, sch_chain);
 779                 n++;
 780                 fsk_detach(fs, flags);
 781         }
 782         ND("done %d flowsets", n);
 783 }
 784
 785 /*
 786  * called on 'queue X delete' -- removes the flowset from fshash,
 787  * deletes all queues for the flowset, and removes the flowset.
 788  */
 789 static int
 790 delete_fs(int i, int locked)
 791 {
 792         struct dn_fsk *fs;
 793         int err = 0;
 794
 795         if (!locked)
 796                 DN_BH_WLOCK();
 797         fs = dn_ht_find(dn_cfg.fshash, i, DNHT_REMOVE, NULL);
 798         ND("fs %d found %p", i, fs);
 799         if (fs) {
 800                 fsk_detach(fs, DN_DETACH | DN_DELETE_FS);
 801                 err = 0;
 802         } else
 803                 err = EINVAL;
 804         if (!locked)
 805                 DN_BH_WUNLOCK();
 806         return err;
 807 }
 808
 809 /*----- end of flowset hashtable support -------------*/
 810
 811 /*------------------------------------------------------------
 812  * Scheduler hash. When searching by index we pass sched_nr,
 813  * otherwise we pass struct dn_sch * which is the first field in
 814  * struct dn_schk so we can cast between the two. We use this trick
 815  * because in the create phase (but it should be fixed).
 816  */
 817 static uint32_t
 818 schk_hash(uintptr_t key, int flags, void *_arg)
 819 {
 820         uint32_t i = !(flags & DNHT_KEY_IS_OBJ) ? key :
 821                 ((struct dn_schk *)key)->sch.sched_nr;
 822         return ( (i>>8)^(i>>4)^i );
 823 }
 824
 825 static int
 826 schk_match(void *obj, uintptr_t key, int flags, void *_arg)
 827 {
 828         struct dn_schk *s = (struct dn_schk *)obj;
 829         int i = !(flags & DNHT_KEY_IS_OBJ) ? key :
 830                 ((struct dn_schk *)key)->sch.sched_nr;
 831         return (s->sch.sched_nr == i);
 832 }
 833
 834 /*
 835  * Create the entry and intialize with the sched hash if needed.
 836  * Leave s->fp unset so we can tell whether a dn_ht_find() returns
 837  * a new object or a previously existing one.
 838  */
 839 static void *
 840 schk_new(uintptr_t key, int flags, void *arg)
 841 {
 842         struct schk_new_arg *a = arg;
 843         struct dn_schk *s;
 844         int l = sizeof(*s) +a->fp->schk_datalen;
 845
 846         s = malloc(l, M_DUMMYNET, M_NOWAIT | M_ZERO);
 847         if (s == NULL)
 848                 return NULL;
 849         set_oid(&s->link.oid, DN_LINK, sizeof(s->link));
 850         s->sch = *a->sch; // copy initial values
 851         s->link.link_nr = s->sch.sched_nr;
 852         SLIST_INIT(&s->fsk_list);
 853         /* initialize the hash table or create the single instance */
 854         s->fp = a->fp;  /* si_new needs this */
 855         s->drain_bucket = 0;
 856         if (s->sch.flags & DN_HAVE_MASK) {
 857                 s->siht = dn_ht_init(NULL, s->sch.buckets,
 858                         offsetof(struct dn_sch_inst, si_next),
 859                         si_hash, si_match, si_new);
 860                 if (s->siht == NULL) {
 861                         free(s, M_DUMMYNET);
 862                         return NULL;
 863                 }
 864         }
 865         s->fp = NULL;   /* mark as a new scheduler */
 866         dn_cfg.schk_count++;
 867         return s;
 868 }
 869
 870 /*
 871  * Callback for sched delete. Notify all attached flowsets to
 872  * detach from the scheduler, destroy the internal flowset, and
 873  * all instances. The scheduler goes away too.
 874  * arg is 0 (only detach flowsets and destroy instances)
 875  * DN_DESTROY (detach & delete queues, delete schk)
 876  * or DN_DELETE_FS (delete queues and flowsets, delete schk)
 877  */
 878 static int
 879 schk_delete_cb(void *obj, void *arg)
 880 {
 881         struct dn_schk *s = obj;
 882 #if 0
 883         int a = (int)arg;
 884         ND("sched %d arg %s%s",
 885                 s->sch.sched_nr,
 886                 a&DN_DESTROY ? "DEL ":"",
 887                 a&DN_DELETE_FS ? "DEL_FS":"");
 888 #endif
 889         fsk_detach_list(&s->fsk_list, arg ? DN_DESTROY : 0);
 890         /* no more flowset pointing to us now */
 891         if (s->sch.flags & DN_HAVE_MASK) {
 892                 dn_ht_scan(s->siht, si_destroy, NULL);
 893                 dn_ht_free(s->siht, 0);
 894         } else if (s->siht)
 895                 si_destroy(s->siht, NULL);
 896         if (s->profile) {
 897                 free(s->profile, M_DUMMYNET);
 898                 s->profile = NULL;
 899         }
 900         s->siht = NULL;
 901         if (s->fp->destroy)
 902                 s->fp->destroy(s);
 903         bzero(s, sizeof(*s));   // safety
 904         free(obj, M_DUMMYNET);
 905         dn_cfg.schk_count--;
 906         return DNHT_SCAN_DEL;
 907 }
 908
 909 /*
 910  * called on a 'sched X delete' command. Deletes a single scheduler.
 911  * This is done by removing from the schedhash, unlinking all
 912  * flowsets and deleting their traffic.
 913  */
 914 static int
 915 delete_schk(int i)
 916 {
 917         struct dn_schk *s;
 918
 919         s = dn_ht_find(dn_cfg.schedhash, i, DNHT_REMOVE, NULL);
 920         ND("%d %p", i, s);
 921         if (!s)
 922                 return EINVAL;
 923         delete_fs(i + DN_MAX_ID, 1); /* first delete internal fs */
 924         /* then detach flowsets, delete traffic */
 925         schk_delete_cb(s, (void*)(uintptr_t)DN_DESTROY);
 926         return 0;
 927 }
 928 /*--- end of schk hashtable support ---*/
 929
 930 static int
 931 copy_obj(char **start, char *end, void *_o, const char *msg, int i)
 932 {
 933         struct dn_id *o = _o;
 934         int have = end - *start;
 935
 936         if (have < o->len || o->len == 0 || o->type == 0) {
 937                 D("(WARN) type %d %s %d have %d need %d",
 938                         o->type, msg, i, have, o->len);
 939                 return 1;
 940         }
 941         ND("type %d %s %d len %d", o->type, msg, i, o->len);
 942         bcopy(_o, *start, o->len);
 943         if (o->type == DN_LINK) {
 944                 /* Adjust burst parameter for link */
 945                 struct dn_link *l = (struct dn_link *)*start;
 946                 l->burst =  div64(l->burst, 8 * hz);
 947                 l->delay = l->delay * 1000 / hz;
 948         } else if (o->type == DN_SCH) {
 949                 /* Set id->id to the number of instances */
 950                 struct dn_schk *s = _o;
 951                 struct dn_id *id = (struct dn_id *)(*start);
 952                 id->id = (s->sch.flags & DN_HAVE_MASK) ?
 953                         dn_ht_entries(s->siht) : (s->siht ? 1 : 0);
 954         }
 955         *start += o->len;
 956         return 0;
 957 }
 958
 959 /* Specific function to copy a queue.
 960  * Copies only the user-visible part of a queue (which is in
 961  * a struct dn_flow), and sets len accordingly.
 962  */
 963 static int
 964 copy_obj_q(char **start, char *end, void *_o, const char *msg, int i)
 965 {
 966         struct dn_id *o = _o;
 967         int have = end - *start;
 968         int len = sizeof(struct dn_flow); /* see above comment */
 969
 970         if (have < len || o->len == 0 || o->type != DN_QUEUE) {
 971                 D("ERROR type %d %s %d have %d need %d",
 972                         o->type, msg, i, have, len);
 973                 return 1;
 974         }
 975         ND("type %d %s %d len %d", o->type, msg, i, len);
 976         bcopy(_o, *start, len);
 977         ((struct dn_id*)(*start))->len = len;
 978         *start += len;
 979         return 0;
 980 }
 981
 982 static int
 983 copy_q_cb(void *obj, void *arg)
 984 {
 985         struct dn_queue *q = obj;
 986         struct copy_args *a = arg;
 987         struct dn_flow *ni = (struct dn_flow *)(*a->start);
 988         if (copy_obj_q(a->start, a->end, &q->ni, "queue", -1))
 989                 return DNHT_SCAN_END;
 990         ni->oid.type = DN_FLOW; /* override the DN_QUEUE */
 991         ni->oid.id = si_hash((uintptr_t)&ni->fid, 0, NULL);
 992         return 0;
 993 }
 994
 995 static int
 996 copy_q(struct copy_args *a, struct dn_fsk *fs, int flags)
 997 {
 998         if (!fs->qht)
 999                 return 0;
1000         if (fs->fs.flags & DN_QHT_HASH)
1001                 dn_ht_scan(fs->qht, copy_q_cb, a);
1002         else
1003                 copy_q_cb(fs->qht, a);
1004         return 0;
1005 }
1006
1007 /*
1008  * This routine only copies the initial part of a profile ? XXX
1009  */
1010 static int
1011 copy_profile(struct copy_args *a, struct dn_profile *p)
1012 {
1013         int have = a->end - *a->start;
1014         /* XXX here we check for max length */
1015         int profile_len = sizeof(struct dn_profile) -
1016                 ED_MAX_SAMPLES_NO*sizeof(int);
1017
1018         if (p == NULL)
1019                 return 0;
1020         if (have < profile_len) {
1021                 D("error have %d need %d", have, profile_len);
1022                 return 1;
1023         }
1024         bcopy(p, *a->start, profile_len);
1025         ((struct dn_id *)(*a->start))->len = profile_len;
1026         *a->start += profile_len;
1027         return 0;
1028 }
1029
1030 static int
1031 copy_flowset(struct copy_args *a, struct dn_fsk *fs, int flags)
1032 {
1033         struct dn_fs *ufs = (struct dn_fs *)(*a->start);
1034         if (!fs)
1035                 return 0;
1036         ND("flowset %d", fs->fs.fs_nr);
1037         if (copy_obj(a->start, a->end, &fs->fs, "flowset", fs->fs.fs_nr))
1038                 return DNHT_SCAN_END;
1039         ufs->oid.id = (fs->fs.flags & DN_QHT_HASH) ?
1040                 dn_ht_entries(fs->qht) : (fs->qht ? 1 : 0);
1041         if (flags) {    /* copy queues */
1042                 copy_q(a, fs, 0);
1043         }
1044         return 0;
1045 }
1046
1047 static int
1048 copy_si_cb(void *obj, void *arg)
1049 {
1050         struct dn_sch_inst *si = obj;
1051         struct copy_args *a = arg;
1052         struct dn_flow *ni = (struct dn_flow *)(*a->start);
1053         if (copy_obj(a->start, a->end, &si->ni, "inst",
1054                         si->sched->sch.sched_nr))
1055                 return DNHT_SCAN_END;
1056         ni->oid.type = DN_FLOW; /* override the DN_SCH_I */
1057         ni->oid.id = si_hash((uintptr_t)si, DNHT_KEY_IS_OBJ, NULL);
1058         return 0;
1059 }
1060
1061 static int
1062 copy_si(struct copy_args *a, struct dn_schk *s, int flags)
1063 {
1064         if (s->sch.flags & DN_HAVE_MASK)
1065                 dn_ht_scan(s->siht, copy_si_cb, a);
1066         else if (s->siht)
1067                 copy_si_cb(s->siht, a);
1068         return 0;
1069 }
1070
1071 /*
1072  * compute a list of children of a scheduler and copy up
1073  */
1074 static int
1075 copy_fsk_list(struct copy_args *a, struct dn_schk *s, int flags)
1076 {
1077         struct dn_fsk *fs;
1078         struct dn_id *o;
1079         uint32_t *p;
1080
1081         int n = 0, space = sizeof(*o);
1082         SLIST_FOREACH(fs, &s->fsk_list, sch_chain) {
1083                 if (fs->fs.fs_nr < DN_MAX_ID)
1084                         n++;
1085         }
1086         space += n * sizeof(uint32_t);
1087         DX(3, "sched %d has %d flowsets", s->sch.sched_nr, n);
1088         if (a->end - *(a->start) < space)
1089                 return DNHT_SCAN_END;
1090         o = (struct dn_id *)(*(a->start));
1091         o->len = space;
1092         *a->start += o->len;
1093         o->type = DN_TEXT;
1094         p = (uint32_t *)(o+1);
1095         SLIST_FOREACH(fs, &s->fsk_list, sch_chain)
1096                 if (fs->fs.fs_nr < DN_MAX_ID)
1097                         *p++ = fs->fs.fs_nr;
1098         return 0;
1099 }
1100
1101 static int
1102 copy_data_helper(void *_o, void *_arg)
1103 {
1104         struct copy_args *a = _arg;
1105         uint32_t *r = a->extra->r; /* start of first range */
1106         uint32_t *lim;  /* first invalid pointer */
1107         int n;
1108
1109         lim = (uint32_t *)((char *)(a->extra) + a->extra->o.len);
1110
1111         if (a->type == DN_LINK || a->type == DN_SCH) {
1112                 /* pipe|sched show, we receive a dn_schk */
1113                 struct dn_schk *s = _o;
1114
1115                 n = s->sch.sched_nr;
1116                 if (a->type == DN_SCH && n >= DN_MAX_ID)
1117                         return 0;       /* not a scheduler */
1118                 if (a->type == DN_LINK && n <= DN_MAX_ID)
1119                     return 0;   /* not a pipe */
1120
1121                 /* see if the object is within one of our ranges */
1122                 for (;r < lim; r += 2) {
1123                         if (n < r[0] || n > r[1])
1124                                 continue;
1125                         /* Found a valid entry, copy and we are done */
1126                         if (a->flags & DN_C_LINK) {
1127                                 if (copy_obj(a->start, a->end,
1128                                     &s->link, "link", n))
1129                                         return DNHT_SCAN_END;
1130                                 if (copy_profile(a, s->profile))
1131                                         return DNHT_SCAN_END;
1132                                 if (copy_flowset(a, s->fs, 0))
1133                                         return DNHT_SCAN_END;
1134                         }
1135                         if (a->flags & DN_C_SCH) {
1136                                 if (copy_obj(a->start, a->end,
1137                                     &s->sch, "sched", n))
1138                                         return DNHT_SCAN_END;
1139                                 /* list all attached flowsets */
1140                                 if (copy_fsk_list(a, s, 0))
1141                                         return DNHT_SCAN_END;
1142                         }
1143                         if (a->flags & DN_C_FLOW)
1144                                 copy_si(a, s, 0);
1145                         break;
1146                 }
1147         } else if (a->type == DN_FS) {
1148                 /* queue show, skip internal flowsets */
1149                 struct dn_fsk *fs = _o;
1150
1151                 n = fs->fs.fs_nr;
1152                 if (n >= DN_MAX_ID)
1153                         return 0;
1154                 /* see if the object is within one of our ranges */
1155                 for (;r < lim; r += 2) {
1156                         if (n < r[0] || n > r[1])
1157                                 continue;
1158                         if (copy_flowset(a, fs, 0))
1159                                 return DNHT_SCAN_END;
1160                         copy_q(a, fs, 0);
1161                         break; /* we are done */
1162                 }
1163         }
1164         return 0;
1165 }
1166
1167 static inline struct dn_schk *
1168 locate_scheduler(int i)
1169 {
1170         return dn_ht_find(dn_cfg.schedhash, i, 0, NULL);
1171 }
1172
1173 /*
1174  * red parameters are in fixed point arithmetic.
1175  */
1176 static int
1177 config_red(struct dn_fsk *fs)
1178 {
1179         int64_t s, idle, weight, w0;
1180         int t, i;
1181
1182         fs->w_q = fs->fs.w_q;
1183         fs->max_p = fs->fs.max_p;
1184         ND("called");
1185         /* Doing stuff that was in userland */
1186         i = fs->sched->link.bandwidth;
1187         s = (i <= 0) ? 0 :
1188                 hz * dn_cfg.red_avg_pkt_size * 8 * SCALE(1) / i;
1189
1190         idle = div64((s * 3) , fs->w_q); /* s, fs->w_q scaled; idle not scaled */
1191         fs->lookup_step = div64(idle , dn_cfg.red_lookup_depth);
1192         /* fs->lookup_step not scaled, */
1193         if (!fs->lookup_step)
1194                 fs->lookup_step = 1;
1195         w0 = weight = SCALE(1) - fs->w_q; //fs->w_q scaled
1196
1197         for (t = fs->lookup_step; t > 1; --t)
1198                 weight = SCALE_MUL(weight, w0);
1199         fs->lookup_weight = (int)(weight); // scaled
1200
1201         /* Now doing stuff that was in kerneland */
1202         fs->min_th = SCALE(fs->fs.min_th);
1203         fs->max_th = SCALE(fs->fs.max_th);
1204
1205         if (fs->fs.max_th == fs->fs.min_th)
1206                 fs->c_1 = fs->max_p;
1207         else
1208                 fs->c_1 = SCALE((int64_t)(fs->max_p)) / (fs->fs.max_th - fs->fs.min_th);
1209         fs->c_2 = SCALE_MUL(fs->c_1, SCALE(fs->fs.min_th));
1210
1211         if (fs->fs.flags & DN_IS_GENTLE_RED) {
1212                 fs->c_3 = (SCALE(1) - fs->max_p) / fs->fs.max_th;
1213                 fs->c_4 = SCALE(1) - 2 * fs->max_p;
1214         }
1215
1216         /* If the lookup table already exist, free and create it again. */
1217         if (fs->w_q_lookup) {
1218                 free(fs->w_q_lookup, M_DUMMYNET);
1219                 fs->w_q_lookup = NULL;
1220         }
1221         if (dn_cfg.red_lookup_depth == 0) {
1222                 printf("\ndummynet: net.inet.ip.dummynet.red_lookup_depth"
1223                     "must be > 0\n");
1224                 fs->fs.flags &= ~DN_IS_RED;
1225                 fs->fs.flags &= ~DN_IS_GENTLE_RED;
1226                 return (EINVAL);
1227         }
1228         fs->lookup_depth = dn_cfg.red_lookup_depth;
1229         fs->w_q_lookup = (u_int *)malloc(fs->lookup_depth * sizeof(int),
1230             M_DUMMYNET, M_NOWAIT);
1231         if (fs->w_q_lookup == NULL) {
1232                 printf("dummynet: sorry, cannot allocate red lookup table\n");
1233                 fs->fs.flags &= ~DN_IS_RED;
1234                 fs->fs.flags &= ~DN_IS_GENTLE_RED;
1235                 return(ENOSPC);
1236         }
1237
1238         /* Fill the lookup table with (1 - w_q)^x */
1239         fs->w_q_lookup[0] = SCALE(1) - fs->w_q;
1240
1241         for (i = 1; i < fs->lookup_depth; i++)
1242                 fs->w_q_lookup[i] =
1243                     SCALE_MUL(fs->w_q_lookup[i - 1], fs->lookup_weight);
1244
1245         if (dn_cfg.red_avg_pkt_size < 1)
1246                 dn_cfg.red_avg_pkt_size = 512;
1247         fs->avg_pkt_size = dn_cfg.red_avg_pkt_size;
1248         if (dn_cfg.red_max_pkt_size < 1)
1249                 dn_cfg.red_max_pkt_size = 1500;
1250         fs->max_pkt_size = dn_cfg.red_max_pkt_size;
1251         ND("exit");
1252         return 0;
1253 }
1254
1255 /* Scan all flowset attached to this scheduler and update red */
1256 static void
1257 update_red(struct dn_schk *s)
1258 {
1259         struct dn_fsk *fs;
1260         SLIST_FOREACH(fs, &s->fsk_list, sch_chain) {
1261                 if (fs && (fs->fs.flags & DN_IS_RED))
1262                         config_red(fs);
1263         }
1264 }
1265
1266 /* attach flowset to scheduler s, possibly requeue */
1267 static void
1268 fsk_attach(struct dn_fsk *fs, struct dn_schk *s)
1269 {
1270         ND("remove fs %d from fsunlinked, link to sched %d",
1271                 fs->fs.fs_nr, s->sch.sched_nr);
1272         SLIST_REMOVE(&dn_cfg.fsu, fs, dn_fsk, sch_chain);
1273         fs->sched = s;
1274         SLIST_INSERT_HEAD(&s->fsk_list, fs, sch_chain);
1275         if (s->fp->new_fsk)
1276                 s->fp->new_fsk(fs);
1277         /* XXX compute fsk_mask */
1278         fs->fsk_mask = fs->fs.flow_mask;
1279         if (fs->sched->sch.flags & DN_HAVE_MASK)
1280                 flow_id_or(&fs->sched->sch.sched_mask, &fs->fsk_mask);
1281         if (fs->qht) {
1282                 /*
1283                  * we must drain qht according to the old
1284                  * type, and reinsert according to the new one.
1285                  * The requeue is complex -- in general we need to
1286                  * reclassify every single packet.
1287                  * For the time being, let's hope qht is never set
1288                  * when we reach this point.
1289                  */
1290                 D("XXX TODO requeue from fs %d to sch %d",
1291                         fs->fs.fs_nr, s->sch.sched_nr);
1292                 fs->qht = NULL;
1293         }
1294         /* set the new type for qht */
1295         if (nonzero_mask(&fs->fsk_mask))
1296                 fs->fs.flags |= DN_QHT_HASH;
1297         else
1298                 fs->fs.flags &= ~DN_QHT_HASH;
1299
1300         /* XXX config_red() can fail... */
1301         if (fs->fs.flags & DN_IS_RED)
1302                 config_red(fs);
1303 }
1304
1305 /* update all flowsets which may refer to this scheduler */
1306 static void
1307 update_fs(struct dn_schk *s)
1308 {
1309         struct dn_fsk *fs, *tmp;
1310
1311         SLIST_FOREACH_SAFE(fs, &dn_cfg.fsu, sch_chain, tmp) {
1312                 if (s->sch.sched_nr != fs->fs.sched_nr) {
1313                         D("fs %d for sch %d not %d still unlinked",
1314                                 fs->fs.fs_nr, fs->fs.sched_nr,
1315                                 s->sch.sched_nr);
1316                         continue;
1317                 }
1318                 fsk_attach(fs, s);
1319         }
1320 }
1321
1322 #ifdef NEW_AQM
1323 /* Retrieve AQM configurations to ipfw userland
1324  */
1325 static int
1326 get_aqm_parms(struct sockopt *sopt)
1327 {
1328         struct dn_extra_parms  *ep;
1329         struct dn_fsk *fs;
1330         size_t sopt_valsize;
1331         int l, err = 0;
1332
1333         sopt_valsize = sopt->sopt_valsize;
1334         l = sizeof(*ep);
1335         if (sopt->sopt_valsize < l) {
1336                 D("bad len sopt->sopt_valsize %d len %d",
1337                         (int) sopt->sopt_valsize , l);
1338                 err = EINVAL;
1339                 return err;
1340         }
1341         ep = malloc(l, M_DUMMYNET, M_WAITOK);
1342         if(!ep) {
1343                 err = ENOMEM ;
1344                 return err;
1345         }
1346         do {
1347                 err = sooptcopyin(sopt, ep, l, l);
1348                 if(err)
1349                         break;
1350                 sopt->sopt_valsize = sopt_valsize;
1351                 if (ep->oid.len < l) {
1352                         err = EINVAL;
1353                         break;
1354                 }
1355
1356                 fs = dn_ht_find(dn_cfg.fshash, ep->nr, 0, NULL);
1357                 if (!fs) {
1358                         D("fs %d not found", ep->nr);
1359                         err = EINVAL;
1360                         break;
1361                 }
1362
1363                 if (fs->aqmfp && fs->aqmfp->getconfig) {
1364                         if(fs->aqmfp->getconfig(fs, ep)) {
1365                                 D("Error while trying to get AQM params");
1366                                 err = EINVAL;
1367                                 break;
1368                         }
1369                         ep->oid.len = l;
1370                         err = sooptcopyout(sopt, ep, l);
1371                 }
1372         }while(0);
1373
1374         free(ep, M_DUMMYNET);
1375         return err;
1376 }
1377
1378 /* Retrieve AQM configurations to ipfw userland
1379  */
1380 static int
1381 get_sched_parms(struct sockopt *sopt)
1382 {
1383         struct dn_extra_parms  *ep;
1384         struct dn_schk *schk;
1385         size_t sopt_valsize;
1386         int l, err = 0;
1387
1388         sopt_valsize = sopt->sopt_valsize;
1389         l = sizeof(*ep);
1390         if (sopt->sopt_valsize < l) {
1391                 D("bad len sopt->sopt_valsize %d len %d",
1392                         (int) sopt->sopt_valsize , l);
1393                 err = EINVAL;
1394                 return err;
1395         }
1396         ep = malloc(l, M_DUMMYNET, M_WAITOK);
1397         if(!ep) {
1398                 err = ENOMEM ;
1399                 return err;
1400         }
1401         do {
1402                 err = sooptcopyin(sopt, ep, l, l);
1403                 if(err)
1404                         break;
1405                 sopt->sopt_valsize = sopt_valsize;
1406                 if (ep->oid.len < l) {
1407                         err = EINVAL;
1408                         break;
1409                 }
1410
1411                 schk = locate_scheduler(ep->nr);
1412                 if (!schk) {
1413                         D("sched %d not found", ep->nr);
1414                         err = EINVAL;
1415                         break;
1416                 }
1417
1418                 if (schk->fp && schk->fp->getconfig) {
1419                         if(schk->fp->getconfig(schk, ep)) {
1420                                 D("Error while trying to get sched params");
1421                                 err = EINVAL;
1422                                 break;
1423                         }
1424                         ep->oid.len = l;
1425                         err = sooptcopyout(sopt, ep, l);
1426                 }
1427         }while(0);
1428         free(ep, M_DUMMYNET);
1429
1430         return err;
1431 }
1432
1433 /* Configure AQM for flowset 'fs'.
1434  * extra parameters are passed from userland.
1435  */
1436 static int
1437 config_aqm(struct dn_fsk *fs, struct  dn_extra_parms *ep, int busy)
1438 {
1439         int err = 0;
1440
1441         do {
1442                 /* no configurations */
1443                 if (!ep) {
1444                         err = 0;
1445                         break;
1446                 }
1447
1448                 /* no AQM for this flowset*/
1449                 if (!strcmp(ep->name,"")) {
1450                         err = 0;
1451                         break;
1452                 }
1453                 if (ep->oid.len < sizeof(*ep)) {
1454                         D("short aqm len %d", ep->oid.len);
1455                                 err = EINVAL;
1456                                 break;
1457                 }
1458
1459                 if (busy) {
1460                         D("Unable to configure flowset, flowset busy!");
1461                         err = EINVAL;
1462                         break;
1463                 }
1464
1465                 /* deconfigure old aqm if exist */
1466                 if (fs->aqmcfg && fs->aqmfp && fs->aqmfp->deconfig) {
1467                         aqm_cleanup_deconfig_fs(fs);
1468                 }
1469
1470                 if (!(fs->aqmfp = find_aqm_type(0, ep->name))) {
1471                         D("AQM functions not found for type %s!", ep->name);
1472                         fs->fs.flags &= ~DN_IS_AQM;
1473                         err = EINVAL;
1474                         break;
1475                 } else
1476                         fs->fs.flags |= DN_IS_AQM;
1477
1478                 if (ep->oid.subtype != DN_AQM_PARAMS) {
1479                                 D("Wrong subtype");
1480                                 err = EINVAL;
1481                                 break;
1482                 }
1483
1484                 if (fs->aqmfp->config) {
1485                         err = fs->aqmfp->config(fs, ep, ep->oid.len);
1486                         if (err) {
1487                                         D("Unable to configure AQM for FS %d", fs->fs.fs_nr );
1488                                         fs->fs.flags &= ~DN_IS_AQM;
1489                                         fs->aqmfp = NULL;
1490                                         break;
1491                         }
1492                 }
1493         } while(0);
1494
1495         return err;
1496 }
1497 #endif
1498
1499 /*
1500  * Configuration -- to preserve backward compatibility we use
1501  * the following scheme (N is 65536)
1502  *      NUMBER          SCHED   LINK    FLOWSET
1503  *         1 ..  N-1    (1)WFQ  (2)WFQ  (3)queue
1504  *       N+1 .. 2N-1    (4)FIFO (5)FIFO (6)FIFO for sched 1..N-1
1505  *      2N+1 .. 3N-1    --      --      (7)FIFO for sched N+1..2N-1
1506  *
1507  * "pipe i config" configures #1, #2 and #3
1508  * "sched i config" configures #1 and possibly #6
1509  * "queue i config" configures #3
1510  * #1 is configured with 'pipe i config' or 'sched i config'
1511  * #2 is configured with 'pipe i config', and created if not
1512  *      existing with 'sched i config'
1513  * #3 is configured with 'queue i config'
1514  * #4 is automatically configured after #1, can only be FIFO
1515  * #5 is automatically configured after #2
1516  * #6 is automatically created when #1 is !MULTIQUEUE,
1517  *      and can be updated.
1518  * #7 is automatically configured after #2
1519  */
1520
1521 /*
1522  * configure a link (and its FIFO instance)
1523  */
1524 static int
1525 config_link(struct dn_link *p, struct dn_id *arg)
1526 {
1527         int i;
1528
1529         if (p->oid.len != sizeof(*p)) {
1530                 D("invalid pipe len %d", p->oid.len);
1531                 return EINVAL;
1532         }
1533         i = p->link_nr;
1534         if (i <= 0 || i >= DN_MAX_ID)
1535                 return EINVAL;
1536         /*
1537          * The config program passes parameters as follows:
1538          * bw = bits/second (0 means no limits),
1539          * delay = ms, must be translated into ticks.
1540          * qsize = slots/bytes
1541          * burst ???
1542          */
1543         p->delay = (p->delay * hz) / 1000;
1544         /* Scale burst size: bytes -> bits * hz */
1545         p->burst *= 8 * hz;
1546
1547         DN_BH_WLOCK();
1548         /* do it twice, base link and FIFO link */
1549         for (; i < 2*DN_MAX_ID; i += DN_MAX_ID) {
1550             struct dn_schk *s = locate_scheduler(i);
1551             if (s == NULL) {
1552                 DN_BH_WUNLOCK();
1553                 D("sched %d not found", i);
1554                 return EINVAL;
1555             }
1556             /* remove profile if exists */
1557             if (s->profile) {
1558                 free(s->profile, M_DUMMYNET);
1559                 s->profile = NULL;
1560             }
1561             /* copy all parameters */
1562             s->link.oid = p->oid;
1563             s->link.link_nr = i;
1564             s->link.delay = p->delay;
1565             if (s->link.bandwidth != p->bandwidth) {
1566                 /* XXX bandwidth changes, need to update red params */
1567             s->link.bandwidth = p->bandwidth;
1568                 update_red(s);
1569             }
1570             s->link.burst = p->burst;
1571             schk_reset_credit(s);
1572         }
1573         dn_cfg.id++;
1574         DN_BH_WUNLOCK();
1575         return 0;
1576 }
1577
1578 /*
1579  * configure a flowset. Can be called from inside with locked=1,
1580  */
1581 static struct dn_fsk *
1582 config_fs(struct dn_fs *nfs, struct dn_id *arg, int locked)
1583 {
1584         int i;
1585         struct dn_fsk *fs;
1586
1587         if (nfs->oid.len != sizeof(*nfs)) {
1588                 D("invalid flowset len %d", nfs->oid.len);
1589                 return NULL;
1590         }
1591         i = nfs->fs_nr;
1592         if (i <= 0 || i >= 3*DN_MAX_ID)
1593                 return NULL;
1594         ND("flowset %d", i);
1595         /* XXX other sanity checks */
1596         if (nfs->flags & DN_QSIZE_BYTES) {
1597                 ipdn_bound_var(&nfs->qsize, 16384,
1598                     1500, dn_cfg.byte_limit, NULL); // "queue byte size");
1599         } else {
1600                 ipdn_bound_var(&nfs->qsize, 50,
1601                     1, dn_cfg.slot_limit, NULL); // "queue slot size");
1602         }
1603         if (nfs->flags & DN_HAVE_MASK) {
1604                 /* make sure we have some buckets */
1605                 ipdn_bound_var((int *)&nfs->buckets, dn_cfg.hash_size,
1606                         1, dn_cfg.max_hash_size, "flowset buckets");
1607         } else {
1608                 nfs->buckets = 1;       /* we only need 1 */
1609         }
1610         if (!locked)
1611                 DN_BH_WLOCK();
1612         do { /* exit with break when done */
1613             struct dn_schk *s;
1614             int flags = nfs->sched_nr ? DNHT_INSERT : 0;
1615             int j;
1616             int oldc = dn_cfg.fsk_count;
1617             fs = dn_ht_find(dn_cfg.fshash, i, flags, NULL);
1618             if (fs == NULL) {
1619                 D("missing sched for flowset %d", i);
1620                 break;
1621             }
1622             /* grab some defaults from the existing one */
1623             if (nfs->sched_nr == 0) /* reuse */
1624                 nfs->sched_nr = fs->fs.sched_nr;
1625             for (j = 0; j < sizeof(nfs->par)/sizeof(nfs->par[0]); j++) {
1626                 if (nfs->par[j] == -1) /* reuse */
1627                     nfs->par[j] = fs->fs.par[j];
1628             }
1629             if (bcmp(&fs->fs, nfs, sizeof(*nfs)) == 0) {
1630                 ND("flowset %d unchanged", i);
1631 #ifdef NEW_AQM
1632                 /* reconfigure AQM as the parameters can be changed.
1633                  * we consider the flowsetis  busy if it has scheduler instance(s)
1634                 */
1635                 s = locate_scheduler(nfs->sched_nr);
1636                 config_aqm(fs, (struct dn_extra_parms *) arg,
1637                         s != NULL && s->siht != NULL);
1638 #endif
1639                 break; /* no change, nothing to do */
1640             }
1641             if (oldc != dn_cfg.fsk_count)       /* new item */
1642                 dn_cfg.id++;
1643             s = locate_scheduler(nfs->sched_nr);
1644             /* detach from old scheduler if needed, preserving
1645              * queues if we need to reattach. Then update the
1646              * configuration, and possibly attach to the new sched.
1647              */
1648             DX(2, "fs %d changed sched %d@%p to %d@%p",
1649                 fs->fs.fs_nr,
1650                 fs->fs.sched_nr, fs->sched, nfs->sched_nr, s);
1651             if (fs->sched) {
1652                 int flags = s ? DN_DETACH : (DN_DETACH | DN_DESTROY);
1653                 flags |= DN_DESTROY; /* XXX temporary */
1654                 fsk_detach(fs, flags);
1655             }
1656             fs->fs = *nfs; /* copy configuration */
1657 #ifdef NEW_AQM
1658                         fs->aqmfp = NULL;
1659                         config_aqm(fs, (struct dn_extra_parms *) arg, s != NULL && s->siht != NULL);
1660 #endif
1661             if (s != NULL)
1662                 fsk_attach(fs, s);
1663         } while (0);
1664         if (!locked)
1665                 DN_BH_WUNLOCK();
1666         return fs;
1667 }
1668
1669 /*
1670  * config/reconfig a scheduler and its FIFO variant.
1671  * For !MULTIQUEUE schedulers, also set up the flowset.
1672  *
1673  * On reconfigurations (detected because s->fp is set),
1674  * detach existing flowsets preserving traffic, preserve link,
1675  * and delete the old scheduler creating a new one.
1676  */
1677 static int
1678 config_sched(struct dn_sch *_nsch, struct dn_id *arg)
1679 {
1680         struct dn_schk *s;
1681         struct schk_new_arg a; /* argument for schk_new */
1682         int i;
1683         struct dn_link p;       /* copy of oldlink */
1684         struct dn_profile *pf = NULL;   /* copy of old link profile */
1685         /* Used to preserv mask parameter */
1686         struct ipfw_flow_id new_mask;
1687         int new_buckets = 0;
1688         int new_flags = 0;
1689         int pipe_cmd;
1690         int err = ENOMEM;
1691
1692         a.sch = _nsch;
1693         if (a.sch->oid.len != sizeof(*a.sch)) {
1694                 D("bad sched len %d", a.sch->oid.len);
1695                 return EINVAL;
1696         }
1697         i = a.sch->sched_nr;
1698         if (i <= 0 || i >= DN_MAX_ID)
1699                 return EINVAL;
1700         /* make sure we have some buckets */
1701         if (a.sch->flags & DN_HAVE_MASK)
1702                 ipdn_bound_var((int *)&a.sch->buckets, dn_cfg.hash_size,
1703                         1, dn_cfg.max_hash_size, "sched buckets");
1704         /* XXX other sanity checks */
1705         bzero(&p, sizeof(p));
1706
1707         pipe_cmd = a.sch->flags & DN_PIPE_CMD;
1708         a.sch->flags &= ~DN_PIPE_CMD; //XXX do it even if is not set?
1709         if (pipe_cmd) {
1710                 /* Copy mask parameter */
1711                 new_mask = a.sch->sched_mask;
1712                 new_buckets = a.sch->buckets;
1713                 new_flags = a.sch->flags;
1714         }
1715         DN_BH_WLOCK();
1716 again: /* run twice, for wfq and fifo */
1717         /*
1718          * lookup the type. If not supplied, use the previous one
1719          * or default to WF2Q+. Otherwise, return an error.
1720          */
1721         dn_cfg.id++;
1722         a.fp = find_sched_type(a.sch->oid.subtype, a.sch->name);
1723         if (a.fp != NULL) {
1724                 /* found. Lookup or create entry */
1725                 s = dn_ht_find(dn_cfg.schedhash, i, DNHT_INSERT, &a);
1726         } else if (a.sch->oid.subtype == 0 && !a.sch->name[0]) {
1727                 /* No type. search existing s* or retry with WF2Q+ */
1728                 s = dn_ht_find(dn_cfg.schedhash, i, 0, &a);
1729                 if (s != NULL) {
1730                         a.fp = s->fp;
1731                         /* Scheduler exists, skip to FIFO scheduler
1732                          * if command was pipe config...
1733                          */
1734                         if (pipe_cmd)
1735                                 goto next;
1736                 } else {
1737                         /* New scheduler, create a wf2q+ with no mask
1738                          * if command was pipe config...
1739                          */
1740                         if (pipe_cmd) {
1741                                 /* clear mask parameter */
1742                                 bzero(&a.sch->sched_mask, sizeof(new_mask));
1743                                 a.sch->buckets = 0;
1744                                 a.sch->flags &= ~DN_HAVE_MASK;
1745                         }
1746                         a.sch->oid.subtype = DN_SCHED_WF2QP;
1747                         goto again;
1748                 }
1749         } else {
1750                 D("invalid scheduler type %d %s",
1751                         a.sch->oid.subtype, a.sch->name);
1752                 err = EINVAL;
1753                 goto error;
1754         }
1755         /* normalize name and subtype */
1756         a.sch->oid.subtype = a.fp->type;
1757         bzero(a.sch->name, sizeof(a.sch->name));
1758         strlcpy(a.sch->name, a.fp->name, sizeof(a.sch->name));
1759         if (s == NULL) {
1760                 D("cannot allocate scheduler %d", i);
1761                 goto error;
1762         }
1763         /* restore existing link if any */
1764         if (p.link_nr) {
1765                 s->link = p;
1766                 if (!pf || pf->link_nr != p.link_nr) { /* no saved value */
1767                         s->profile = NULL; /* XXX maybe not needed */
1768                 } else {
1769                         s->profile = malloc(sizeof(struct dn_profile),
1770                                              M_DUMMYNET, M_NOWAIT | M_ZERO);
1771                         if (s->profile == NULL) {
1772                                 D("cannot allocate profile");
1773                                 goto error; //XXX
1774                         }
1775                         bcopy(pf, s->profile, sizeof(*pf));
1776                 }
1777         }
1778         p.link_nr = 0;
1779         if (s->fp == NULL) {
1780                 DX(2, "sched %d new type %s", i, a.fp->name);
1781         } else if (s->fp != a.fp ||
1782                         bcmp(a.sch, &s->sch, sizeof(*a.sch)) ) {
1783                 /* already existing. */
1784                 DX(2, "sched %d type changed from %s to %s",
1785                         i, s->fp->name, a.fp->name);
1786                 DX(4, "   type/sub %d/%d -> %d/%d",
1787                         s->sch.oid.type, s->sch.oid.subtype,
1788                         a.sch->oid.type, a.sch->oid.subtype);
1789                 if (s->link.link_nr == 0)
1790                         D("XXX WARNING link 0 for sched %d", i);
1791                 p = s->link;    /* preserve link */
1792                 if (s->profile) {/* preserve profile */
1793                         if (!pf)
1794                                 pf = malloc(sizeof(*pf),
1795                                     M_DUMMYNET, M_NOWAIT | M_ZERO);
1796                         if (pf) /* XXX should issue a warning otherwise */
1797                                 bcopy(s->profile, pf, sizeof(*pf));
1798                 }
1799                 /* remove from the hash */
1800                 dn_ht_find(dn_cfg.schedhash, i, DNHT_REMOVE, NULL);
1801                 /* Detach flowsets, preserve queues. */
1802                 // schk_delete_cb(s, NULL);
1803                 // XXX temporarily, kill queues
1804                 schk_delete_cb(s, (void *)DN_DESTROY);
1805                 goto again;
1806         } else {
1807                 DX(4, "sched %d unchanged type %s", i, a.fp->name);
1808         }
1809         /* complete initialization */
1810         s->sch = *a.sch;
1811         s->fp = a.fp;
1812         s->cfg = arg;
1813         // XXX schk_reset_credit(s);
1814         /* create the internal flowset if needed,
1815          * trying to reuse existing ones if available
1816          */
1817         if (!(s->fp->flags & DN_MULTIQUEUE) && !s->fs) {
1818                 s->fs = dn_ht_find(dn_cfg.fshash, i, 0, NULL);
1819                 if (!s->fs) {
1820                         struct dn_fs fs;
1821                         bzero(&fs, sizeof(fs));
1822                         set_oid(&fs.oid, DN_FS, sizeof(fs));
1823                         fs.fs_nr = i + DN_MAX_ID;
1824                         fs.sched_nr = i;
1825                         s->fs = config_fs(&fs, NULL, 1 /* locked */);
1826                 }
1827                 if (!s->fs) {
1828                         schk_delete_cb(s, (void *)DN_DESTROY);
1829                         D("error creating internal fs for %d", i);
1830                         goto error;
1831                 }
1832         }
1833         /* call init function after the flowset is created */
1834         if (s->fp->config)
1835                 s->fp->config(s);
1836         update_fs(s);
1837 next:
1838         if (i < DN_MAX_ID) { /* now configure the FIFO instance */
1839                 i += DN_MAX_ID;
1840                 if (pipe_cmd) {
1841                         /* Restore mask parameter for FIFO */
1842                         a.sch->sched_mask = new_mask;
1843                         a.sch->buckets = new_buckets;
1844                         a.sch->flags = new_flags;
1845                 } else {
1846                         /* sched config shouldn't modify the FIFO scheduler */
1847                         if (dn_ht_find(dn_cfg.schedhash, i, 0, &a) != NULL) {
1848                                 /* FIFO already exist, don't touch it */
1849                                 err = 0; /* and this is not an error */
1850                                 goto error;
1851                         }
1852                 }
1853                 a.sch->sched_nr = i;
1854                 a.sch->oid.subtype = DN_SCHED_FIFO;
1855                 bzero(a.sch->name, sizeof(a.sch->name));
1856                 goto again;
1857         }
1858         err = 0;
1859 error:
1860         DN_BH_WUNLOCK();
1861         if (pf)
1862                 free(pf, M_DUMMYNET);
1863         return err;
1864 }
1865
1866 /*
1867  * attach a profile to a link
1868  */
1869 static int
1870 config_profile(struct dn_profile *pf, struct dn_id *arg)
1871 {
1872         struct dn_schk *s;
1873         int i, olen, err = 0;
1874
1875         if (pf->oid.len < sizeof(*pf)) {
1876                 D("short profile len %d", pf->oid.len);
1877                 return EINVAL;
1878         }
1879         i = pf->link_nr;
1880         if (i <= 0 || i >= DN_MAX_ID)
1881                 return EINVAL;
1882         /* XXX other sanity checks */
1883         DN_BH_WLOCK();
1884         for (; i < 2*DN_MAX_ID; i += DN_MAX_ID) {
1885                 s = locate_scheduler(i);
1886
1887                 if (s == NULL) {
1888                         err = EINVAL;
1889                         break;
1890                 }
1891                 dn_cfg.id++;
1892                 /*
1893                  * If we had a profile and the new one does not fit,
1894                  * or it is deleted, then we need to free memory.
1895                  */
1896                 if (s->profile && (pf->samples_no == 0 ||
1897                     s->profile->oid.len < pf->oid.len)) {
1898                         free(s->profile, M_DUMMYNET);
1899                         s->profile = NULL;
1900                 }
1901                 if (pf->samples_no == 0)
1902                         continue;
1903                 /*
1904                  * new profile, possibly allocate memory
1905                  * and copy data.
1906                  */
1907                 if (s->profile == NULL)
1908                         s->profile = malloc(pf->oid.len,
1909                                     M_DUMMYNET, M_NOWAIT | M_ZERO);
1910                 if (s->profile == NULL) {
1911                         D("no memory for profile %d", i);
1912                         err = ENOMEM;
1913                         break;
1914                 }
1915                 /* preserve larger length XXX double check */
1916                 olen = s->profile->oid.len;
1917                 if (olen < pf->oid.len)
1918                         olen = pf->oid.len;
1919                 bcopy(pf, s->profile, pf->oid.len);
1920                 s->profile->oid.len = olen;
1921         }
1922         DN_BH_WUNLOCK();
1923         return err;
1924 }
1925
1926 /*
1927  * Delete all objects:
1928  */
1929 static void
1930 dummynet_flush(void)
1931 {
1932
1933         /* delete all schedulers and related links/queues/flowsets */
1934         dn_ht_scan(dn_cfg.schedhash, schk_delete_cb,
1935                 (void *)(uintptr_t)DN_DELETE_FS);
1936         /* delete all remaining (unlinked) flowsets */
1937         DX(4, "still %d unlinked fs", dn_cfg.fsk_count);
1938         dn_ht_free(dn_cfg.fshash, DNHT_REMOVE);
1939         fsk_detach_list(&dn_cfg.fsu, DN_DELETE_FS);
1940         /* Reinitialize system heap... */
1941         heap_init(&dn_cfg.evheap, 16, offsetof(struct dn_id, id));
1942 }
1943
1944 /*
1945  * Main handler for configuration. We are guaranteed to be called
1946  * with an oid which is at least a dn_id.
1947  * - the first object is the command (config, delete, flush, ...)
1948  * - config_link must be issued after the corresponding config_sched
1949  * - parameters (DN_TXT) for an object must preceed the object
1950  *   processed on a config_sched.
1951  */
1952 int
1953 do_config(void *p, int l)
1954 {
1955         struct dn_id *next, *o;
1956         int err = 0, err2 = 0;
1957         struct dn_id *arg = NULL;
1958         uintptr_t *a;
1959
1960         o = p;
1961         if (o->id != DN_API_VERSION) {
1962                 D("invalid api version got %d need %d",
1963                         o->id, DN_API_VERSION);
1964                 return EINVAL;
1965         }
1966         for (; l >= sizeof(*o); o = next) {
1967                 struct dn_id *prev = arg;
1968                 if (o->len < sizeof(*o) || l < o->len) {
1969                         D("bad len o->len %d len %d", o->len, l);
1970                         err = EINVAL;
1971                         break;
1972                 }
1973                 l -= o->len;
1974                 next = (struct dn_id *)((char *)o + o->len);
1975                 err = 0;
1976                 switch (o->type) {
1977                 default:
1978                         D("cmd %d not implemented", o->type);
1979                         break;
1980
1981 #ifdef EMULATE_SYSCTL
1982                 /* sysctl emulation.
1983                  * if we recognize the command, jump to the correct
1984                  * handler and return
1985                  */
1986                 case DN_SYSCTL_SET:
1987                         err = kesysctl_emu_set(p, l);
1988                         return err;
1989 #endif
1990
1991                 case DN_CMD_CONFIG: /* simply a header */
1992                         break;
1993
1994                 case DN_CMD_DELETE:
1995                         /* the argument is in the first uintptr_t after o */
1996                         a = (uintptr_t *)(o+1);
1997                         if (o->len < sizeof(*o) + sizeof(*a)) {
1998                                 err = EINVAL;
1999                                 break;
2000                         }
2001                         switch (o->subtype) {
2002                         case DN_LINK:
2003                                 /* delete base and derived schedulers */
2004                                 DN_BH_WLOCK();
2005                                 err = delete_schk(*a);
2006                                 err2 = delete_schk(*a + DN_MAX_ID);
2007                                 DN_BH_WUNLOCK();
2008                                 if (!err)
2009                                         err = err2;
2010                                 break;
2011
2012                         default:
2013                                 D("invalid delete type %d",
2014                                         o->subtype);
2015                                 err = EINVAL;
2016                                 break;
2017
2018                         case DN_FS:
2019                                 err = (*a <1 || *a >= DN_MAX_ID) ?
2020                                         EINVAL : delete_fs(*a, 0) ;
2021                                 break;
2022                         }
2023                         break;
2024
2025                 case DN_CMD_FLUSH:
2026                         DN_BH_WLOCK();
2027                         dummynet_flush();
2028                         DN_BH_WUNLOCK();
2029                         break;
2030                 case DN_TEXT:   /* store argument the next block */
2031                         prev = NULL;
2032                         arg = o;
2033                         break;
2034                 case DN_LINK:
2035                         err = config_link((struct dn_link *)o, arg);
2036                         break;
2037                 case DN_PROFILE:
2038                         err = config_profile((struct dn_profile *)o, arg);
2039                         break;
2040                 case DN_SCH:
2041                         err = config_sched((struct dn_sch *)o, arg);
2042                         break;
2043                 case DN_FS:
2044                         err = (NULL==config_fs((struct dn_fs *)o, arg, 0));
2045                         break;
2046                 }
2047                 if (prev)
2048                         arg = NULL;
2049                 if (err != 0)
2050                         break;
2051         }
2052         return err;
2053 }
2054
2055 static int
2056 compute_space(struct dn_id *cmd, struct copy_args *a)
2057 {
2058         int x = 0, need = 0;
2059         int profile_size = sizeof(struct dn_profile) -
2060                 ED_MAX_SAMPLES_NO*sizeof(int);
2061
2062         /* NOTE about compute space:
2063          * NP   = dn_cfg.schk_count
2064          * NSI  = dn_cfg.si_count
2065          * NF   = dn_cfg.fsk_count
2066          * NQ   = dn_cfg.queue_count
2067          * - ipfw pipe show
2068          *   (NP/2)*(dn_link + dn_sch + dn_id + dn_fs) only half scheduler
2069          *                             link, scheduler template, flowset
2070          *                             integrated in scheduler and header
2071          *                             for flowset list
2072          *   (NSI)*(dn_flow) all scheduler instance (includes
2073          *                              the queue instance)
2074          * - ipfw sched show
2075          *   (NP/2)*(dn_link + dn_sch + dn_id + dn_fs) only half scheduler
2076          *                             link, scheduler template, flowset
2077          *                             integrated in scheduler and header
2078          *                             for flowset list
2079          *   (NSI * dn_flow) all scheduler instances
2080          *   (NF * sizeof(uint_32)) space for flowset list linked to scheduler
2081          *   (NQ * dn_queue) all queue [XXXfor now not listed]
2082          * - ipfw queue show
2083          *   (NF * dn_fs) all flowset
2084          *   (NQ * dn_queue) all queues
2085          */
2086         switch (cmd->subtype) {
2087         default:
2088                 return -1;
2089         /* XXX where do LINK and SCH differ ? */
2090         /* 'ipfw sched show' could list all queues associated to
2091          * a scheduler. This feature for now is disabled
2092          */
2093         case DN_LINK:   /* pipe show */
2094                 x = DN_C_LINK | DN_C_SCH | DN_C_FLOW;
2095                 need += dn_cfg.schk_count *
2096                         (sizeof(struct dn_fs) + profile_size) / 2;
2097                 need += dn_cfg.fsk_count * sizeof(uint32_t);
2098                 break;
2099         case DN_SCH:    /* sched show */
2100                 need += dn_cfg.schk_count *
2101                         (sizeof(struct dn_fs) + profile_size) / 2;
2102                 need += dn_cfg.fsk_count * sizeof(uint32_t);
2103                 x = DN_C_SCH | DN_C_LINK | DN_C_FLOW;
2104                 break;
2105         case DN_FS:     /* queue show */
2106                 x = DN_C_FS | DN_C_QUEUE;
2107                 break;
2108         case DN_GET_COMPAT:     /* compatibility mode */
2109                 need =  dn_compat_calc_size();
2110                 break;
2111         }
2112         a->flags = x;
2113         if (x & DN_C_SCH) {
2114                 need += dn_cfg.schk_count * sizeof(struct dn_sch) / 2;
2115                 /* NOT also, each fs might be attached to a sched */
2116                 need += dn_cfg.schk_count * sizeof(struct dn_id) / 2;
2117         }
2118         if (x & DN_C_FS)
2119                 need += dn_cfg.fsk_count * sizeof(struct dn_fs);
2120         if (x & DN_C_LINK) {
2121                 need += dn_cfg.schk_count * sizeof(struct dn_link) / 2;
2122         }
2123         /*
2124          * When exporting a queue to userland, only pass up the
2125          * struct dn_flow, which is the only visible part.
2126          */
2127
2128         if (x & DN_C_QUEUE)
2129                 need += dn_cfg.queue_count * sizeof(struct dn_flow);
2130         if (x & DN_C_FLOW)
2131                 need += dn_cfg.si_count * (sizeof(struct dn_flow));
2132         return need;
2133 }
2134
2135 /*
2136  * If compat != NULL dummynet_get is called in compatibility mode.
2137  * *compat will be the pointer to the buffer to pass to ipfw
2138  */
2139 int
2140 dummynet_get(struct sockopt *sopt, void **compat)
2141 {
2142         int have, i, need, error;
2143         char *start = NULL, *buf;
2144         size_t sopt_valsize;
2145         struct dn_id *cmd;
2146         struct copy_args a;
2147         struct copy_range r;
2148         int l = sizeof(struct dn_id);
2149
2150         bzero(&a, sizeof(a));
2151         bzero(&r, sizeof(r));
2152
2153         /* save and restore original sopt_valsize around copyin */
2154         sopt_valsize = sopt->sopt_valsize;
2155
2156         cmd = &r.o;
2157
2158         if (!compat) {
2159                 /* copy at least an oid, and possibly a full object */
2160                 error = sooptcopyin(sopt, cmd, sizeof(r), sizeof(*cmd));
2161                 sopt->sopt_valsize = sopt_valsize;
2162                 if (error)
2163                         goto done;
2164                 l = cmd->len;
2165 #ifdef EMULATE_SYSCTL
2166                 /* sysctl emulation. */
2167                 if (cmd->type == DN_SYSCTL_GET)
2168                         return kesysctl_emu_get(sopt);
2169 #endif
2170                 if (l > sizeof(r)) {
2171                         /* request larger than default, allocate buffer */
2172                         cmd = malloc(l,  M_DUMMYNET, M_WAITOK);
2173                         error = sooptcopyin(sopt, cmd, l, l);
2174                         sopt->sopt_valsize = sopt_valsize;
2175                         if (error)
2176                                 goto done;
2177                 }
2178         } else { /* compatibility */
2179                 error = 0;
2180                 cmd->type = DN_CMD_GET;
2181                 cmd->len = sizeof(struct dn_id);
2182                 cmd->subtype = DN_GET_COMPAT;
2183                 // cmd->id = sopt_valsize;
2184                 D("compatibility mode");
2185         }
2186
2187 #ifdef NEW_AQM
2188         /* get AQM params */
2189         if(cmd->subtype == DN_AQM_PARAMS) {
2190                 error = get_aqm_parms(sopt);
2191                 goto done;
2192         /* get Scheduler params */
2193         } else if (cmd->subtype == DN_SCH_PARAMS) {
2194                 error = get_sched_parms(sopt);
2195                 goto done;
2196         }
2197 #endif
2198
2199         a.extra = (struct copy_range *)cmd;
2200         if (cmd->len == sizeof(*cmd)) { /* no range, create a default */
2201                 uint32_t *rp = (uint32_t *)(cmd + 1);
2202                 cmd->len += 2* sizeof(uint32_t);
2203                 rp[0] = 1;
2204                 rp[1] = DN_MAX_ID - 1;
2205                 if (cmd->subtype == DN_LINK) {
2206                         rp[0] += DN_MAX_ID;
2207                         rp[1] += DN_MAX_ID;
2208                 }
2209         }
2210         /* Count space (under lock) and allocate (outside lock).
2211          * Exit with lock held if we manage to get enough buffer.
2212          * Try a few times then give up.
2213          */
2214         for (have = 0, i = 0; i < 10; i++) {
2215                 DN_BH_WLOCK();
2216                 need = compute_space(cmd, &a);
2217
2218                 /* if there is a range, ignore value from compute_space() */
2219                 if (l > sizeof(*cmd))
2220                         need = sopt_valsize - sizeof(*cmd);
2221
2222                 if (need < 0) {
2223                         DN_BH_WUNLOCK();
2224                         error = EINVAL;
2225                         goto done;
2226                 }
2227                 need += sizeof(*cmd);
2228                 cmd->id = need;
2229                 if (have >= need)
2230                         break;
2231
2232                 DN_BH_WUNLOCK();
2233                 if (start)
2234                         free(start, M_DUMMYNET);
2235                 start = NULL;
2236                 if (need > sopt_valsize)
2237                         break;
2238
2239                 have = need;
2240                 start = malloc(have, M_DUMMYNET, M_WAITOK | M_ZERO);
2241         }
2242
2243         if (start == NULL) {
2244                 if (compat) {
2245                         *compat = NULL;
2246                         error =  1; // XXX
2247                 } else {
2248                         error = sooptcopyout(sopt, cmd, sizeof(*cmd));
2249                 }
2250                 goto done;
2251         }
2252         ND("have %d:%d sched %d, %d:%d links %d, %d:%d flowsets %d, "
2253                 "%d:%d si %d, %d:%d queues %d",
2254                 dn_cfg.schk_count, sizeof(struct dn_sch), DN_SCH,
2255                 dn_cfg.schk_count, sizeof(struct dn_link), DN_LINK,
2256                 dn_cfg.fsk_count, sizeof(struct dn_fs), DN_FS,
2257                 dn_cfg.si_count, sizeof(struct dn_flow), DN_SCH_I,
2258                 dn_cfg.queue_count, sizeof(struct dn_queue), DN_QUEUE);
2259         sopt->sopt_valsize = sopt_valsize;
2260         a.type = cmd->subtype;
2261
2262         if (compat == NULL) {
2263                 bcopy(cmd, start, sizeof(*cmd));
2264                 ((struct dn_id*)(start))->len = sizeof(struct dn_id);
2265                 buf = start + sizeof(*cmd);
2266         } else
2267                 buf = start;
2268         a.start = &buf;
2269         a.end = start + have;
2270         /* start copying other objects */
2271         if (compat) {
2272                 a.type = DN_COMPAT_PIPE;
2273                 dn_ht_scan(dn_cfg.schedhash, copy_data_helper_compat, &a);
2274                 a.type = DN_COMPAT_QUEUE;
2275                 dn_ht_scan(dn_cfg.fshash, copy_data_helper_compat, &a);
2276         } else if (a.type == DN_FS) {
2277                 dn_ht_scan(dn_cfg.fshash, copy_data_helper, &a);
2278         } else {
2279                 dn_ht_scan(dn_cfg.schedhash, copy_data_helper, &a);
2280         }
2281         DN_BH_WUNLOCK();
2282
2283         if (compat) {
2284                 *compat = start;
2285                 sopt->sopt_valsize = buf - start;
2286                 /* free() is done by ip_dummynet_compat() */
2287                 start = NULL; //XXX hack
2288         } else {
2289                 error = sooptcopyout(sopt, start, buf - start);
2290         }
2291 done:
2292         if (cmd && cmd != &r.o)
2293                 free(cmd, M_DUMMYNET);
2294         if (start)
2295                 free(start, M_DUMMYNET);
2296         return error;
2297 }
2298
2299 /* Callback called on scheduler instance to delete it if idle */
2300 static int
2301 drain_scheduler_cb(void *_si, void *arg)
2302 {
2303         struct dn_sch_inst *si = _si;
2304
2305         if ((si->kflags & DN_ACTIVE) || si->dline.mq.head != NULL)
2306                 return 0;
2307
2308         if (si->sched->fp->flags & DN_MULTIQUEUE) {
2309                 if (si->q_count == 0)
2310                         return si_destroy(si, NULL);
2311                 else
2312                         return 0;
2313         } else { /* !DN_MULTIQUEUE */
2314                 if ((si+1)->ni.length == 0)
2315                         return si_destroy(si, NULL);
2316                 else
2317                         return 0;
2318         }
2319         return 0; /* unreachable */
2320 }
2321
2322 /* Callback called on scheduler to check if it has instances */
2323 static int
2324 drain_scheduler_sch_cb(void *_s, void *arg)
2325 {
2326         struct dn_schk *s = _s;
2327
2328         if (s->sch.flags & DN_HAVE_MASK) {
2329                 dn_ht_scan_bucket(s->siht, &s->drain_bucket,
2330                                 drain_scheduler_cb, NULL);
2331                 s->drain_bucket++;
2332         } else {
2333                 if (s->siht) {
2334                         if (drain_scheduler_cb(s->siht, NULL) == DNHT_SCAN_DEL)
2335                                 s->siht = NULL;
2336                 }
2337         }
2338         return 0;
2339 }
2340
2341 /* Called every tick, try to delete a 'bucket' of scheduler */
2342 void
2343 dn_drain_scheduler(void)
2344 {
2345         dn_ht_scan_bucket(dn_cfg.schedhash, &dn_cfg.drain_sch,
2346                            drain_scheduler_sch_cb, NULL);
2347         dn_cfg.drain_sch++;
2348 }
2349
2350 /* Callback called on queue to delete if it is idle */
2351 static int
2352 drain_queue_cb(void *_q, void *arg)
2353 {
2354         struct dn_queue *q = _q;
2355
2356         if (q->ni.length == 0) {
2357                 dn_delete_queue(q, DN_DESTROY);
2358                 return DNHT_SCAN_DEL; /* queue is deleted */
2359         }
2360
2361         return 0; /* queue isn't deleted */
2362 }
2363
2364 /* Callback called on flowset used to check if it has queues */
2365 static int
2366 drain_queue_fs_cb(void *_fs, void *arg)
2367 {
2368         struct dn_fsk *fs = _fs;
2369
2370         if (fs->fs.flags & DN_QHT_HASH) {
2371                 /* Flowset has a hash table for queues */
2372                 dn_ht_scan_bucket(fs->qht, &fs->drain_bucket,
2373                                 drain_queue_cb, NULL);
2374                 fs->drain_bucket++;
2375         } else {
2376                 /* No hash table for this flowset, null the pointer
2377                  * if the queue is deleted
2378                  */
2379                 if (fs->qht) {
2380                         if (drain_queue_cb(fs->qht, NULL) == DNHT_SCAN_DEL)
2381                                 fs->qht = NULL;
2382                 }
2383         }
2384         return 0;
2385 }
2386
2387 /* Called every tick, try to delete a 'bucket' of queue */
2388 void
2389 dn_drain_queue(void)
2390 {
2391         /* scan a bucket of flowset */
2392         dn_ht_scan_bucket(dn_cfg.fshash, &dn_cfg.drain_fs,
2393                                drain_queue_fs_cb, NULL);
2394         dn_cfg.drain_fs++;
2395 }
2396
2397 /*
2398  * Handler for the various dummynet socket options
2399  */
2400 static int
2401 ip_dn_ctl(struct sockopt *sopt)
2402 {
2403         void *p = NULL;
2404         int error, l;
2405
2406         error = priv_check(sopt->sopt_td, PRIV_NETINET_DUMMYNET);
2407         if (error)
2408                 return (error);
2409
2410         /* Disallow sets in really-really secure mode. */
2411         if (sopt->sopt_dir == SOPT_SET) {
2412                 error =  securelevel_ge(sopt->sopt_td->td_ucred, 3);
2413                 if (error)
2414                         return (error);
2415         }
2416
2417         switch (sopt->sopt_name) {
2418         default :
2419                 D("dummynet: unknown option %d", sopt->sopt_name);
2420                 error = EINVAL;
2421                 break;
2422
2423         case IP_DUMMYNET_FLUSH:
2424         case IP_DUMMYNET_CONFIGURE:
2425         case IP_DUMMYNET_DEL:   /* remove a pipe or queue */
2426         case IP_DUMMYNET_GET:
2427                 D("dummynet: compat option %d", sopt->sopt_name);
2428                 error = ip_dummynet_compat(sopt);
2429                 break;
2430
2431         case IP_DUMMYNET3 :
2432                 if (sopt->sopt_dir == SOPT_GET) {
2433                         error = dummynet_get(sopt, NULL);
2434                         break;
2435                 }
2436                 l = sopt->sopt_valsize;
2437                 if (l < sizeof(struct dn_id) || l > 12000) {
2438                         D("argument len %d invalid", l);
2439                         break;
2440                 }
2441                 p = malloc(l, M_TEMP, M_WAITOK); // XXX can it fail ?
2442                 error = sooptcopyin(sopt, p, l, l);
2443                 if (error)
2444                         break ;
2445                 error = do_config(p, l);
2446                 break;
2447         }
2448
2449         if (p != NULL)
2450                 free(p, M_TEMP);
2451
2452         return error ;
2453 }
2454
2455
2456 static void
2457 ip_dn_init(void)
2458 {
2459         if (dn_cfg.init_done)
2460                 return;
2461         printf("DUMMYNET %p with IPv6 initialized (100409)\n", curvnet);
2462         dn_cfg.init_done = 1;
2463         /* Set defaults here. MSVC does not accept initializers,
2464          * and this is also useful for vimages
2465          */
2466         /* queue limits */
2467         dn_cfg.slot_limit = 100; /* Foot shooting limit for queues. */
2468         dn_cfg.byte_limit = 1024 * 1024;
2469         dn_cfg.expire = 1;
2470
2471         /* RED parameters */
2472         dn_cfg.red_lookup_depth = 256;  /* default lookup table depth */
2473         dn_cfg.red_avg_pkt_size = 512;  /* default medium packet size */
2474         dn_cfg.red_max_pkt_size = 1500; /* default max packet size */
2475
2476         /* hash tables */
2477         dn_cfg.max_hash_size = 65536;   /* max in the hash tables */
2478         dn_cfg.hash_size = 64;          /* default hash size */
2479
2480         /* create hash tables for schedulers and flowsets.
2481          * In both we search by key and by pointer.
2482          */
2483         dn_cfg.schedhash = dn_ht_init(NULL, dn_cfg.hash_size,
2484                 offsetof(struct dn_schk, schk_next),
2485                 schk_hash, schk_match, schk_new);
2486         dn_cfg.fshash = dn_ht_init(NULL, dn_cfg.hash_size,
2487                 offsetof(struct dn_fsk, fsk_next),
2488                 fsk_hash, fsk_match, fsk_new);
2489
2490         /* bucket index to drain object */
2491         dn_cfg.drain_fs = 0;
2492         dn_cfg.drain_sch = 0;
2493
2494         heap_init(&dn_cfg.evheap, 16, offsetof(struct dn_id, id));
2495         SLIST_INIT(&dn_cfg.fsu);
2496         SLIST_INIT(&dn_cfg.schedlist);
2497
2498         DN_LOCK_INIT();
2499
2500         TASK_INIT(&dn_task, 0, dummynet_task, curvnet);
2501         dn_tq = taskqueue_create_fast("dummynet", M_WAITOK,
2502             taskqueue_thread_enqueue, &dn_tq);
2503         taskqueue_start_threads(&dn_tq, 1, PI_NET, "dummynet");
2504
2505         callout_init(&dn_timeout, CALLOUT_MPSAFE);
2506         dn_reschedule();
2507
2508         /* Initialize curr_time adjustment mechanics. */
2509         getmicrouptime(&dn_cfg.prev_t);
2510 }
2511
2512 static void
2513 ip_dn_destroy(int last)
2514 {
2515         DN_BH_WLOCK();
2516         /* ensure no more callouts are started */
2517         dn_gone = 1;
2518
2519         /* check for last */
2520         if (last) {
2521                 ND("removing last instance\n");
2522                 ip_dn_ctl_ptr = NULL;
2523                 ip_dn_io_ptr = NULL;
2524         }
2525
2526         dummynet_flush();
2527         DN_BH_WUNLOCK();
2528
2529         callout_drain(&dn_timeout);
2530         taskqueue_drain(dn_tq, &dn_task);
2531         taskqueue_free(dn_tq);
2532
2533         dn_ht_free(dn_cfg.schedhash, 0);
2534         dn_ht_free(dn_cfg.fshash, 0);
2535         heap_free(&dn_cfg.evheap);
2536
2537         DN_LOCK_DESTROY();
2538 }
2539
2540 static int
2541 dummynet_modevent(module_t mod, int type, void *data)
2542 {
2543
2544         if (type == MOD_LOAD) {
2545                 if (ip_dn_io_ptr) {
2546                         printf("DUMMYNET already loaded\n");
2547                         return EEXIST ;
2548                 }
2549                 ip_dn_init();
2550                 ip_dn_ctl_ptr = ip_dn_ctl;
2551                 ip_dn_io_ptr = dummynet_io;
2552                 return 0;
2553         } else if (type == MOD_UNLOAD) {
2554                 ip_dn_destroy(1 /* last */);
2555                 return 0;
2556         } else
2557                 return EOPNOTSUPP;
2558 }
2559
2560 /* modevent helpers for the modules */
2561 static int
2562 load_dn_sched(struct dn_alg *d)
2563 {
2564         struct dn_alg *s;
2565
2566         if (d == NULL)
2567                 return 1; /* error */
2568         ip_dn_init();   /* just in case, we need the lock */
2569
2570         /* Check that mandatory funcs exists */
2571         if (d->enqueue == NULL || d->dequeue == NULL) {
2572                 D("missing enqueue or dequeue for %s", d->name);
2573                 return 1;
2574         }
2575
2576         /* Search if scheduler already exists */
2577         DN_BH_WLOCK();
2578         SLIST_FOREACH(s, &dn_cfg.schedlist, next) {
2579                 if (strcmp(s->name, d->name) == 0) {
2580                         D("%s already loaded", d->name);
2581                         break; /* scheduler already exists */
2582                 }
2583         }
2584         if (s == NULL)
2585                 SLIST_INSERT_HEAD(&dn_cfg.schedlist, d, next);
2586         DN_BH_WUNLOCK();
2587         D("dn_sched %s %sloaded", d->name, s ? "not ":"");
2588         return s ? 1 : 0;
2589 }
2590
2591 static int
2592 unload_dn_sched(struct dn_alg *s)
2593 {
2594         struct dn_alg *tmp, *r;
2595         int err = EINVAL;
2596
2597         ND("called for %s", s->name);
2598
2599         DN_BH_WLOCK();
2600         SLIST_FOREACH_SAFE(r, &dn_cfg.schedlist, next, tmp) {
2601                 if (strcmp(s->name, r->name) != 0)
2602                         continue;
2603                 ND("ref_count = %d", r->ref_count);
2604                 err = (r->ref_count != 0) ? EBUSY : 0;
2605                 if (err == 0)
2606                         SLIST_REMOVE(&dn_cfg.schedlist, r, dn_alg, next);
2607                 break;
2608         }
2609         DN_BH_WUNLOCK();
2610         D("dn_sched %s %sunloaded", s->name, err ? "not ":"");
2611         return err;
2612 }
2613
2614 int
2615 dn_sched_modevent(module_t mod, int cmd, void *arg)
2616 {
2617         struct dn_alg *sch = arg;
2618
2619         if (cmd == MOD_LOAD)
2620                 return load_dn_sched(sch);
2621         else if (cmd == MOD_UNLOAD)
2622                 return unload_dn_sched(sch);
2623         else
2624                 return EINVAL;
2625 }
2626
2627 static moduledata_t dummynet_mod = {
2628         "dummynet", dummynet_modevent, NULL
2629 };
2630
2631 #define DN_SI_SUB       SI_SUB_PROTO_IFATTACHDOMAIN
2632 #define DN_MODEV_ORD    (SI_ORDER_ANY - 128) /* after ipfw */
2633 DECLARE_MODULE(dummynet, dummynet_mod, DN_SI_SUB, DN_MODEV_ORD);
2634 MODULE_DEPEND(dummynet, ipfw, 2, 2, 2);
2635 MODULE_VERSION(dummynet, 3);
2636
2637 /*
2638  * Starting up. Done in order after dummynet_modevent() has been called.
2639  * VNET_SYSINIT is also called for each existing vnet and each new vnet.
2640  */
2641 //VNET_SYSINIT(vnet_dn_init, DN_SI_SUB, DN_MODEV_ORD+2, ip_dn_init, NULL);
2642
2643 /*
2644  * Shutdown handlers up shop. These are done in REVERSE ORDER, but still
2645  * after dummynet_modevent() has been called. Not called on reboot.
2646  * VNET_SYSUNINIT is also called for each exiting vnet as it exits.
2647  * or when the module is unloaded.
2648  */
2649 //VNET_SYSUNINIT(vnet_dn_uninit, DN_SI_SUB, DN_MODEV_ORD+2, ip_dn_destroy, NULL);
2650
2651 #ifdef NEW_AQM
2652
2653 /* modevent helpers for the AQM modules */
2654 static int
2655 load_dn_aqm(struct dn_aqm *d)
2656 {
2657         struct dn_aqm *aqm=NULL;
2658
2659         if (d == NULL)
2660                 return 1; /* error */
2661         ip_dn_init();   /* just in case, we need the lock */
2662
2663         /* Check that mandatory funcs exists */
2664         if (d->enqueue == NULL || d->dequeue == NULL) {
2665                 D("missing enqueue or dequeue for %s", d->name);
2666                 return 1;
2667         }
2668
2669         /* Search if AQM already exists */
2670         DN_BH_WLOCK();
2671         SLIST_FOREACH(aqm, &dn_cfg.aqmlist, next) {
2672                 if (strcmp(aqm->name, d->name) == 0) {
2673                         D("%s already loaded", d->name);
2674                         break; /* AQM already exists */
2675                 }
2676         }
2677         if (aqm == NULL)
2678                 SLIST_INSERT_HEAD(&dn_cfg.aqmlist, d, next);
2679         DN_BH_WUNLOCK();
2680         D("dn_aqm %s %sloaded", d->name, aqm ? "not ":"");
2681         return aqm ? 1 : 0;
2682 }
2683
2684
2685 /* Callback to clean up AQM status for queues connected to a flowset
2686  * and then deconfigure the flowset.
2687  * This function is called before an AQM module is unloaded
2688  */
2689 static int
2690 fs_cleanup(void *_fs, void *arg)
2691 {
2692         struct dn_fsk *fs = _fs;
2693         uint32_t type = *(uint32_t *)arg;
2694
2695         if (fs->aqmfp && fs->aqmfp->type == type)
2696                 aqm_cleanup_deconfig_fs(fs);
2697
2698         return 0;
2699 }
2700
2701 static int
2702 unload_dn_aqm(struct dn_aqm *aqm)
2703 {
2704         struct dn_aqm *tmp, *r;
2705         int err = EINVAL;
2706         err = 0;
2707         ND("called for %s", aqm->name);
2708
2709         DN_BH_WLOCK();
2710
2711         /* clean up AQM status and deconfig flowset */
2712         dn_ht_scan(dn_cfg.fshash, fs_cleanup, &aqm->type);
2713
2714         SLIST_FOREACH_SAFE(r, &dn_cfg.aqmlist, next, tmp) {
2715                 if (strcmp(aqm->name, r->name) != 0)
2716                         continue;
2717                 ND("ref_count = %d", r->ref_count);
2718                 err = (r->ref_count != 0 || r->cfg_ref_count != 0) ? EBUSY : 0;
2719                 if (err == 0)
2720                         SLIST_REMOVE(&dn_cfg.aqmlist, r, dn_aqm, next);
2721                 break;
2722         }
2723         DN_BH_WUNLOCK();
2724         D("%s %sunloaded", aqm->name, err ? "not ":"");
2725         if (err)
2726                 D("ref_count=%d, cfg_ref_count=%d", r->ref_count, r->cfg_ref_count);
2727         return err;
2728 }
2729
2730 int
2731 dn_aqm_modevent(module_t mod, int cmd, void *arg)
2732 {
2733         struct dn_aqm *aqm = arg;
2734
2735         if (cmd == MOD_LOAD)
2736                 return load_dn_aqm(aqm);
2737         else if (cmd == MOD_UNLOAD)
2738                 return unload_dn_aqm(aqm);
2739         else
2740                 return EINVAL;
2741 }
2742 #endif
2743
2744 /* end of file */
2745