sys/netgraph7/ng_car.c

   1 /*-
   2  * Copyright (c) 2005 Nuno Antunes <nuno.antunes@gmail.com>
   3  * Copyright (c) 2007 Alexander Motin <mav@freebsd.org>
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  *
  27  * $FreeBSD: src/sys/netgraph/ng_car.c,v 1.7 2008/03/30 07:53:51 mav Exp $
  28  * $DragonFly: src/sys/netgraph7/ng_car.c,v 1.2 2008/06/26 23:05:35 dillon Exp $
  29  */
  30
  31 /*
  32  * ng_car - An implementation of commited access rate for netgraph
  33  *
  34  * TODO:
  35  *      - Sanitize input config values (impose some limits)
  36  *      - Implement internal packet painting (possibly using mbuf tags)
  37  *      - Implement color-aware mode
  38  *      - Implement DSCP marking for IPv4
  39  */
  40
  41 #include <sys/param.h>
  42 #include <sys/errno.h>
  43 #include <sys/kernel.h>
  44 #include <sys/malloc.h>
  45 #include <sys/mbuf.h>
  46
  47 #include "ng_message.h"
  48 #include "ng_parse.h"
  49 #include "netgraph.h"
  50 #include "ng_car.h"
  51
  52 #define NG_CAR_QUEUE_SIZE       100     /* Maximum queue size for SHAPE mode */
  53 #define NG_CAR_QUEUE_MIN_TH     8       /* Minimum RED threshhold for SHAPE mode */
  54
  55 /* Hook private info */
  56 struct hookinfo {
  57         hook_p          hook;           /* this (source) hook */
  58         hook_p          dest;           /* destination hook */
  59
  60         int64_t         tc;             /* commited token bucket counter */
  61         int64_t         te;             /* exceeded/peak token bucket counter */
  62         struct bintime  lastRefill;     /* last token refill time */
  63
  64         struct ng_car_hookconf conf;    /* hook configuration */
  65         struct ng_car_hookstats stats;  /* hook stats */
  66
  67         struct mbuf     *q[NG_CAR_QUEUE_SIZE];  /* circular packet queue */
  68         u_int           q_first;        /* first queue element */
  69         u_int           q_last;         /* last queue element */
  70         struct callout  q_callout;      /* periodic queue processing routine */
  71         struct mtx      q_mtx;          /* queue mutex */
  72 };
  73
  74 /* Private information for each node instance */
  75 struct privdata {
  76         node_p node;                            /* the node itself */
  77         struct hookinfo upper;                  /* hook to upper layers */
  78         struct hookinfo lower;                  /* hook to lower layers */
  79 };
  80 typedef struct privdata *priv_p;
  81
  82 static ng_constructor_t ng_car_constructor;
  83 static ng_rcvmsg_t      ng_car_rcvmsg;
  84 static ng_shutdown_t    ng_car_shutdown;
  85 static ng_newhook_t     ng_car_newhook;
  86 static ng_rcvdata_t     ng_car_rcvdata;
  87 static ng_disconnect_t  ng_car_disconnect;
  88
  89 static void     ng_car_refillhook(struct hookinfo *h);
  90 static void     ng_car_schedule(struct hookinfo *h);
  91 void            ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2);
  92 static void     ng_car_enqueue(struct hookinfo *h, item_p item);
  93
  94 /* Parse type for struct ng_car_hookstats */
  95 static const struct ng_parse_struct_field ng_car_hookstats_type_fields[]
  96         = NG_CAR_HOOKSTATS;
  97 static const struct ng_parse_type ng_car_hookstats_type = {
  98         &ng_parse_struct_type,
  99         &ng_car_hookstats_type_fields
 100 };
 101
 102 /* Parse type for struct ng_car_bulkstats */
 103 static const struct ng_parse_struct_field ng_car_bulkstats_type_fields[]
 104         = NG_CAR_BULKSTATS(&ng_car_hookstats_type);
 105 static const struct ng_parse_type ng_car_bulkstats_type = {
 106         &ng_parse_struct_type,
 107         &ng_car_bulkstats_type_fields
 108 };
 109
 110 /* Parse type for struct ng_car_hookconf */
 111 static const struct ng_parse_struct_field ng_car_hookconf_type_fields[]
 112         = NG_CAR_HOOKCONF;
 113 static const struct ng_parse_type ng_car_hookconf_type = {
 114         &ng_parse_struct_type,
 115         &ng_car_hookconf_type_fields
 116 };
 117
 118 /* Parse type for struct ng_car_bulkconf */
 119 static const struct ng_parse_struct_field ng_car_bulkconf_type_fields[]
 120         = NG_CAR_BULKCONF(&ng_car_hookconf_type);
 121 static const struct ng_parse_type ng_car_bulkconf_type = {
 122         &ng_parse_struct_type,
 123         &ng_car_bulkconf_type_fields
 124 };
 125
 126 /* Command list */
 127 static struct ng_cmdlist ng_car_cmdlist[] = {
 128         {
 129           NGM_CAR_COOKIE,
 130           NGM_CAR_GET_STATS,
 131           "getstats",
 132           NULL,
 133           &ng_car_bulkstats_type,
 134         },
 135         {
 136           NGM_CAR_COOKIE,
 137           NGM_CAR_CLR_STATS,
 138           "clrstats",
 139           NULL,
 140           NULL,
 141         },
 142         {
 143           NGM_CAR_COOKIE,
 144           NGM_CAR_GETCLR_STATS,
 145           "getclrstats",
 146           NULL,
 147           &ng_car_bulkstats_type,
 148         },
 149
 150         {
 151           NGM_CAR_COOKIE,
 152           NGM_CAR_GET_CONF,
 153           "getconf",
 154           NULL,
 155           &ng_car_bulkconf_type,
 156         },
 157         {
 158           NGM_CAR_COOKIE,
 159           NGM_CAR_SET_CONF,
 160           "setconf",
 161           &ng_car_bulkconf_type,
 162           NULL,
 163         },
 164         { 0 }
 165 };
 166
 167 /* Netgraph node type descriptor */
 168 static struct ng_type ng_car_typestruct = {
 169         .version =      NG_ABI_VERSION,
 170         .name =         NG_CAR_NODE_TYPE,
 171         .constructor =  ng_car_constructor,
 172         .rcvmsg =       ng_car_rcvmsg,
 173         .shutdown =     ng_car_shutdown,
 174         .newhook =      ng_car_newhook,
 175         .rcvdata =      ng_car_rcvdata,
 176         .disconnect =   ng_car_disconnect,
 177         .cmdlist =      ng_car_cmdlist,
 178 };
 179 NETGRAPH_INIT(car, &ng_car_typestruct);
 180
 181 /*
 182  * Node constructor
 183  */
 184 static int
 185 ng_car_constructor(node_p node)
 186 {
 187         priv_p priv;
 188
 189         /* Initialize private descriptor. */
 190         priv = kmalloc(sizeof(*priv), M_NETGRAPH, M_WAITOK | M_NULLOK | M_ZERO);
 191         if (priv == NULL)
 192                 return (ENOMEM);
 193
 194         NG_NODE_SET_PRIVATE(node, priv);
 195         priv->node = node;
 196
 197         /*
 198          * Arbitrary default values
 199          */
 200
 201         priv->upper.hook = NULL;
 202         priv->upper.dest = NULL;
 203         priv->upper.tc = priv->upper.conf.cbs = NG_CAR_CBS_MIN;
 204         priv->upper.te = priv->upper.conf.ebs = NG_CAR_EBS_MIN;
 205         priv->upper.conf.cir = NG_CAR_CIR_DFLT;
 206         priv->upper.conf.green_action = NG_CAR_ACTION_FORWARD;
 207         priv->upper.conf.yellow_action = NG_CAR_ACTION_FORWARD;
 208         priv->upper.conf.red_action = NG_CAR_ACTION_DROP;
 209         priv->upper.conf.mode = 0;
 210         getbinuptime(&priv->upper.lastRefill);
 211         priv->upper.q_first = 0;
 212         priv->upper.q_last = 0;
 213         ng_callout_init(&priv->upper.q_callout);
 214         mtx_init(&priv->upper.q_mtx, "ng_car_u", NULL, MTX_DEF);
 215
 216         priv->lower.hook = NULL;
 217         priv->lower.dest = NULL;
 218         priv->lower.tc = priv->lower.conf.cbs = NG_CAR_CBS_MIN;
 219         priv->lower.te = priv->lower.conf.ebs = NG_CAR_EBS_MIN;
 220         priv->lower.conf.cir = NG_CAR_CIR_DFLT;
 221         priv->lower.conf.green_action = NG_CAR_ACTION_FORWARD;
 222         priv->lower.conf.yellow_action = NG_CAR_ACTION_FORWARD;
 223         priv->lower.conf.red_action = NG_CAR_ACTION_DROP;
 224         priv->lower.conf.mode = 0;
 225         priv->lower.lastRefill = priv->upper.lastRefill;
 226         priv->lower.q_first = 0;
 227         priv->lower.q_last = 0;
 228         ng_callout_init(&priv->lower.q_callout);
 229         mtx_init(&priv->lower.q_mtx, "ng_car_l", NULL, MTX_DEF);
 230
 231         return (0);
 232 }
 233
 234 /*
 235  * Add a hook.
 236  */
 237 static int
 238 ng_car_newhook(node_p node, hook_p hook, const char *name)
 239 {
 240         const priv_p priv = NG_NODE_PRIVATE(node);
 241
 242         if (strcmp(name, NG_CAR_HOOK_LOWER) == 0) {
 243                 priv->lower.hook = hook;
 244                 priv->upper.dest = hook;
 245                 bzero(&priv->lower.stats, sizeof(priv->lower.stats));
 246                 NG_HOOK_SET_PRIVATE(hook, &priv->lower);
 247         } else if (strcmp(name, NG_CAR_HOOK_UPPER) == 0) {
 248                 priv->upper.hook = hook;
 249                 priv->lower.dest = hook;
 250                 bzero(&priv->upper.stats, sizeof(priv->upper.stats));
 251                 NG_HOOK_SET_PRIVATE(hook, &priv->upper);
 252         } else
 253                 return (EINVAL);
 254         return(0);
 255 }
 256
 257 /*
 258  * Data has arrived.
 259  */
 260 static int
 261 ng_car_rcvdata(hook_p hook, item_p item )
 262 {
 263         struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
 264         struct mbuf *m;
 265         int error = 0;
 266         u_int len;
 267
 268         /* If queue is not empty now then enqueue packet. */
 269         if (hinfo->q_first != hinfo->q_last) {
 270                 ng_car_enqueue(hinfo, item);
 271                 return (0);
 272         }
 273
 274         m = NGI_M(item);
 275
 276 #define NG_CAR_PERFORM_MATCH_ACTION(a)                  \
 277         do {                                            \
 278                 switch (a) {                            \
 279                 case NG_CAR_ACTION_FORWARD:             \
 280                         /* Do nothing. */               \
 281                         break;                          \
 282                 case NG_CAR_ACTION_MARK:                \
 283                         /* XXX find a way to mark packets (mbuf tag?) */ \
 284                         ++hinfo->stats.errors;          \
 285                         break;                          \
 286                 case NG_CAR_ACTION_DROP:                \
 287                 default:                                \
 288                         /* Drop packet and return. */   \
 289                         NG_FREE_ITEM(item);             \
 290                         ++hinfo->stats.droped_pkts;     \
 291                         return (0);                     \
 292                 }                                       \
 293         } while (0)
 294
 295         /* Packet is counted as 128 tokens for better resolution */
 296         if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
 297                 len = 128;
 298         } else {
 299                 len = m->m_pkthdr.len;
 300         }
 301
 302         /* Check commited token bucket. */
 303         if (hinfo->tc - len >= 0) {
 304                 /* This packet is green. */
 305                 ++hinfo->stats.green_pkts;
 306                 hinfo->tc -= len;
 307                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.green_action);
 308         } else {
 309
 310                 /* Refill only if not green without it. */
 311                 ng_car_refillhook(hinfo);
 312
 313                  /* Check commited token bucket again after refill. */
 314                 if (hinfo->tc - len >= 0) {
 315                         /* This packet is green */
 316                         ++hinfo->stats.green_pkts;
 317                         hinfo->tc -= len;
 318                         NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.green_action);
 319
 320                 /* If not green and mode is SHAPE, enqueue packet. */
 321                 } else if (hinfo->conf.mode == NG_CAR_SHAPE) {
 322                         ng_car_enqueue(hinfo, item);
 323                         return (0);
 324
 325                 /* If not green and mode is RED, calculate probability. */
 326                 } else if (hinfo->conf.mode == NG_CAR_RED) {
 327                         /* Is packet is bigger then extended burst? */
 328                         if (len - (hinfo->tc - len) > hinfo->conf.ebs) {
 329                                 /* This packet is definitely red. */
 330                                 ++hinfo->stats.red_pkts;
 331                                 hinfo->te = 0;
 332                                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.red_action);
 333
 334                         /* Use token bucket to simulate RED-like drop
 335                            probability. */
 336                         } else if (hinfo->te + (len - hinfo->tc) <
 337                             hinfo->conf.ebs) {
 338                                 /* This packet is yellow */
 339                                 ++hinfo->stats.yellow_pkts;
 340                                 hinfo->te += len - hinfo->tc;
 341                                 /* Go to negative tokens. */
 342                                 hinfo->tc -= len;
 343                                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.yellow_action);
 344                         } else {
 345                                 /* This packet is probaly red. */
 346                                 ++hinfo->stats.red_pkts;
 347                                 hinfo->te = 0;
 348                                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.red_action);
 349                         }
 350                 /* If not green and mode is SINGLE/DOUBLE RATE. */
 351                 } else {
 352                         /* Check extended token bucket. */
 353                         if (hinfo->te - len >= 0) {
 354                                 /* This packet is yellow */
 355                                 ++hinfo->stats.yellow_pkts;
 356                                 hinfo->te -= len;
 357                                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.yellow_action);
 358                         } else {
 359                                 /* This packet is red */
 360                                 ++hinfo->stats.red_pkts;
 361                                 NG_CAR_PERFORM_MATCH_ACTION(hinfo->conf.red_action);
 362                         }
 363                 }
 364         }
 365
 366 #undef NG_CAR_PERFORM_MATCH_ACTION
 367
 368         NG_FWD_ITEM_HOOK(error, item, hinfo->dest);
 369         if (error != 0)
 370                 ++hinfo->stats.errors;
 371         ++hinfo->stats.passed_pkts;
 372
 373         return (error);
 374 }
 375
 376 /*
 377  * Receive a control message.
 378  */
 379 static int
 380 ng_car_rcvmsg(node_p node, item_p item, hook_p lasthook)
 381 {
 382         const priv_p priv = NG_NODE_PRIVATE(node);
 383         struct ng_mesg *resp = NULL;
 384         int error = 0;
 385         struct ng_mesg *msg;
 386
 387         NGI_GET_MSG(item, msg);
 388         switch (msg->header.typecookie) {
 389         case NGM_CAR_COOKIE:
 390                 switch (msg->header.cmd) {
 391                 case NGM_CAR_GET_STATS:
 392                 case NGM_CAR_GETCLR_STATS:
 393                         {
 394                                 struct ng_car_bulkstats *bstats;
 395
 396                                 NG_MKRESPONSE(resp, msg,
 397                                         sizeof(*bstats), M_WAITOK | M_NULLOK);
 398                                 if (resp == NULL) {
 399                                         error = ENOMEM;
 400                                         break;
 401                                 }
 402                                 bstats = (struct ng_car_bulkstats *)resp->data;
 403
 404                                 bcopy(&priv->upper.stats, &bstats->downstream,
 405                                     sizeof(bstats->downstream));
 406                                 bcopy(&priv->lower.stats, &bstats->upstream,
 407                                     sizeof(bstats->upstream));
 408                         }
 409                         if (msg->header.cmd == NGM_CAR_GET_STATS)
 410                                 break;
 411                 case NGM_CAR_CLR_STATS:
 412                         bzero(&priv->upper.stats,
 413                                 sizeof(priv->upper.stats));
 414                         bzero(&priv->lower.stats,
 415                                 sizeof(priv->lower.stats));
 416                         break;
 417                 case NGM_CAR_GET_CONF:
 418                         {
 419                                 struct ng_car_bulkconf *bconf;
 420
 421                                 NG_MKRESPONSE(resp, msg,
 422                                         sizeof(*bconf), M_WAITOK | M_NULLOK);
 423                                 if (resp == NULL) {
 424                                         error = ENOMEM;
 425                                         break;
 426                                 }
 427                                 bconf = (struct ng_car_bulkconf *)resp->data;
 428
 429                                 bcopy(&priv->upper.conf, &bconf->downstream,
 430                                     sizeof(bconf->downstream));
 431                                 bcopy(&priv->lower.conf, &bconf->upstream,
 432                                     sizeof(bconf->upstream));
 433                                 /* Convert internal 1/(8*128) of pps into pps */
 434                                 if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
 435                                     bconf->downstream.cir /= 1024;
 436                                     bconf->downstream.pir /= 1024;
 437                                     bconf->downstream.cbs /= 128;
 438                                     bconf->downstream.ebs /= 128;
 439                                 }
 440                                 if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
 441                                     bconf->upstream.cir /= 1024;
 442                                     bconf->upstream.pir /= 1024;
 443                                     bconf->upstream.cbs /= 128;
 444                                     bconf->upstream.ebs /= 128;
 445                                 }
 446                         }
 447                         break;
 448                 case NGM_CAR_SET_CONF:
 449                         {
 450                                 struct ng_car_bulkconf *const bconf =
 451                                 (struct ng_car_bulkconf *)msg->data;
 452
 453                                 /* Check for invalid or illegal config. */
 454                                 if (msg->header.arglen != sizeof(*bconf)) {
 455                                         error = EINVAL;
 456                                         break;
 457                                 }
 458                                 /* Convert pps into internal 1/(8*128) of pps */
 459                                 if (bconf->downstream.opt & NG_CAR_COUNT_PACKETS) {
 460                                     bconf->downstream.cir *= 1024;
 461                                     bconf->downstream.pir *= 1024;
 462                                     bconf->downstream.cbs *= 125;
 463                                     bconf->downstream.ebs *= 125;
 464                                 }
 465                                 if (bconf->upstream.opt & NG_CAR_COUNT_PACKETS) {
 466                                     bconf->upstream.cir *= 1024;
 467                                     bconf->upstream.pir *= 1024;
 468                                     bconf->upstream.cbs *= 125;
 469                                     bconf->upstream.ebs *= 125;
 470                                 }
 471                                 if ((bconf->downstream.cir > 1000000000) ||
 472                                     (bconf->downstream.pir > 1000000000) ||
 473                                     (bconf->upstream.cir > 1000000000) ||
 474                                     (bconf->upstream.pir > 1000000000) ||
 475                                     (bconf->downstream.cbs == 0 &&
 476                                         bconf->downstream.ebs == 0) ||
 477                                     (bconf->upstream.cbs == 0 &&
 478                                         bconf->upstream.ebs == 0))
 479                                 {
 480                                         error = EINVAL;
 481                                         break;
 482                                 }
 483                                 if ((bconf->upstream.mode == NG_CAR_SHAPE) &&
 484                                     (bconf->upstream.cir == 0)) {
 485                                         error = EINVAL;
 486                                         break;
 487                                 }
 488                                 if ((bconf->downstream.mode == NG_CAR_SHAPE) &&
 489                                     (bconf->downstream.cir == 0)) {
 490                                         error = EINVAL;
 491                                         break;
 492                                 }
 493
 494                                 /* Copy downstream config. */
 495                                 bcopy(&bconf->downstream, &priv->upper.conf,
 496                                     sizeof(priv->upper.conf));
 497                                 priv->upper.tc = priv->upper.conf.cbs;
 498                                 if (priv->upper.conf.mode == NG_CAR_RED ||
 499                                     priv->upper.conf.mode == NG_CAR_SHAPE) {
 500                                         priv->upper.te = 0;
 501                                 } else {
 502                                         priv->upper.te = priv->upper.conf.ebs;
 503                                 }
 504
 505                                 /* Copy upstream config. */
 506                                 bcopy(&bconf->upstream, &priv->lower.conf,
 507                                     sizeof(priv->lower.conf));
 508                                 priv->lower.tc = priv->lower.conf.cbs;
 509                                 if (priv->lower.conf.mode == NG_CAR_RED ||
 510                                     priv->lower.conf.mode == NG_CAR_SHAPE) {
 511                                         priv->lower.te = 0;
 512                                 } else {
 513                                         priv->lower.te = priv->lower.conf.ebs;
 514                                 }
 515                         }
 516                         break;
 517                 default:
 518                         error = EINVAL;
 519                         break;
 520                 }
 521                 break;
 522         default:
 523                 error = EINVAL;
 524                 break;
 525         }
 526         NG_RESPOND_MSG(error, node, item, resp);
 527         NG_FREE_MSG(msg);
 528         return (error);
 529 }
 530
 531 /*
 532  * Do local shutdown processing.
 533  */
 534 static int
 535 ng_car_shutdown(node_p node)
 536 {
 537         const priv_p priv = NG_NODE_PRIVATE(node);
 538
 539         ng_uncallout(&priv->upper.q_callout, node);
 540         ng_uncallout(&priv->lower.q_callout, node);
 541         mtx_destroy(&priv->upper.q_mtx);
 542         mtx_destroy(&priv->lower.q_mtx);
 543         NG_NODE_UNREF(priv->node);
 544         kfree(priv, M_NETGRAPH);
 545         return (0);
 546 }
 547
 548 /*
 549  * Hook disconnection.
 550  *
 551  * For this type, removal of the last link destroys the node.
 552  */
 553 static int
 554 ng_car_disconnect(hook_p hook)
 555 {
 556         struct hookinfo *const hinfo = NG_HOOK_PRIVATE(hook);
 557         const node_p node = NG_HOOK_NODE(hook);
 558         const priv_p priv = NG_NODE_PRIVATE(node);
 559
 560         if (hinfo) {
 561                 /* Purge queue if not empty. */
 562                 while (hinfo->q_first != hinfo->q_last) {
 563                         NG_FREE_M(hinfo->q[hinfo->q_first]);
 564                         hinfo->q_first++;
 565                         if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
 566                                 hinfo->q_first = 0;
 567                 }
 568                 /* Remove hook refs. */
 569                 if (hinfo->hook == priv->upper.hook)
 570                         priv->lower.dest = NULL;
 571                 else
 572                         priv->upper.dest = NULL;
 573                 hinfo->hook = NULL;
 574         }
 575         /* Already shutting down? */
 576         if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0)
 577             && (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
 578                 ng_rmnode_self(NG_HOOK_NODE(hook));
 579         return (0);
 580 }
 581
 582 /*
 583  * Hook's token buckets refillment.
 584  */
 585 static void
 586 ng_car_refillhook(struct hookinfo *h)
 587 {
 588         struct bintime newt, deltat;
 589         unsigned int deltat_us;
 590
 591         /* Get current time. */
 592         getbinuptime(&newt);
 593
 594         /* Get time delta since last refill. */
 595         deltat = newt;
 596         bintime_sub(&deltat, &h->lastRefill);
 597
 598         /* Time must go forward. */
 599         if (deltat.sec < 0) {
 600             h->lastRefill = newt;
 601             return;
 602         }
 603
 604         /* But not too far forward. */
 605         if (deltat.sec >= 1000) {
 606             deltat_us = (1000 << 20);
 607         } else {
 608             /* convert bintime to the 1/(2^20) of sec */
 609             deltat_us = (deltat.sec << 20) + (deltat.frac >> 44);
 610         }
 611
 612         if (h->conf.mode == NG_CAR_SINGLE_RATE) {
 613                 int64_t delta;
 614                 /* Refill commited token bucket. */
 615                 h->tc += (h->conf.cir * deltat_us) >> 23;
 616                 delta = h->tc - h->conf.cbs;
 617                 if (delta > 0) {
 618                         h->tc = h->conf.cbs;
 619
 620                         /* Refill exceeded token bucket. */
 621                         h->te += delta;
 622                         if (h->te > ((int64_t)h->conf.ebs))
 623                                 h->te = h->conf.ebs;
 624                 }
 625
 626         } else if (h->conf.mode == NG_CAR_DOUBLE_RATE) {
 627                 /* Refill commited token bucket. */
 628                 h->tc += (h->conf.cir * deltat_us) >> 23;
 629                 if (h->tc > ((int64_t)h->conf.cbs))
 630                         h->tc = h->conf.cbs;
 631
 632                 /* Refill peak token bucket. */
 633                 h->te += (h->conf.pir * deltat_us) >> 23;
 634                 if (h->te > ((int64_t)h->conf.ebs))
 635                         h->te = h->conf.ebs;
 636
 637         } else { /* RED or SHAPE mode. */
 638                 /* Refill commited token bucket. */
 639                 h->tc += (h->conf.cir * deltat_us) >> 23;
 640                 if (h->tc > ((int64_t)h->conf.cbs))
 641                         h->tc = h->conf.cbs;
 642         }
 643
 644         /* Remember this moment. */
 645         h->lastRefill = newt;
 646 }
 647
 648 /*
 649  * Schedule callout when we will have required tokens.
 650  */
 651 static void
 652 ng_car_schedule(struct hookinfo *hinfo)
 653 {
 654         int     delay;
 655
 656         delay = (-(hinfo->tc)) * hz * 8 / hinfo->conf.cir + 1;
 657
 658         ng_callout(&hinfo->q_callout, NG_HOOK_NODE(hinfo->hook), hinfo->hook,
 659             delay, &ng_car_q_event, NULL, 0);
 660 }
 661
 662 /*
 663  * Queue processing callout handler.
 664  */
 665 void
 666 ng_car_q_event(node_p node, hook_p hook, void *arg, int arg2)
 667 {
 668         struct hookinfo *hinfo = NG_HOOK_PRIVATE(hook);
 669         struct mbuf     *m;
 670         int             error;
 671
 672         /* Refill tokens for time we have slept. */
 673         ng_car_refillhook(hinfo);
 674
 675         /* If we have some tokens */
 676         while (hinfo->tc >= 0) {
 677
 678                 /* Send packet. */
 679                 m = hinfo->q[hinfo->q_first];
 680                 NG_SEND_DATA_ONLY(error, hinfo->dest, m);
 681                 if (error != 0)
 682                         ++hinfo->stats.errors;
 683                 ++hinfo->stats.passed_pkts;
 684
 685                 /* Get next one. */
 686                 hinfo->q_first++;
 687                 if (hinfo->q_first >= NG_CAR_QUEUE_SIZE)
 688                         hinfo->q_first = 0;
 689
 690                 /* Stop if none left. */
 691                 if (hinfo->q_first == hinfo->q_last)
 692                         break;
 693
 694                 /* If we have more packet, try it. */
 695                 m = hinfo->q[hinfo->q_first];
 696                 if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
 697                         hinfo->tc -= 128;
 698                 } else {
 699                         hinfo->tc -= m->m_pkthdr.len;
 700                 }
 701         }
 702
 703         /* If something left */
 704         if (hinfo->q_first != hinfo->q_last)
 705                 /* Schedule queue processing. */
 706                 ng_car_schedule(hinfo);
 707 }
 708
 709 /*
 710  * Enqueue packet.
 711  */
 712 static void
 713 ng_car_enqueue(struct hookinfo *hinfo, item_p item)
 714 {
 715         struct mbuf     *m;
 716         int             len;
 717
 718         NGI_GET_M(item, m);
 719         NG_FREE_ITEM(item);
 720
 721         /* Lock queue mutex. */
 722         mtx_lock(&hinfo->q_mtx);
 723
 724         /* Calculate used queue length. */
 725         len = hinfo->q_last - hinfo->q_first;
 726         if (len < 0)
 727                 len += NG_CAR_QUEUE_SIZE;
 728
 729         /* If queue is overflowed or we have no RED tokens. */
 730         if ((len >= (NG_CAR_QUEUE_SIZE - 1)) ||
 731             (hinfo->te + len >= NG_CAR_QUEUE_SIZE)) {
 732                 /* Drop packet. */
 733                 ++hinfo->stats.red_pkts;
 734                 ++hinfo->stats.droped_pkts;
 735                 NG_FREE_M(m);
 736
 737                 hinfo->te = 0;
 738         } else {
 739                 /* This packet is yellow. */
 740                 ++hinfo->stats.yellow_pkts;
 741
 742                 /* Enqueue packet. */
 743                 hinfo->q[hinfo->q_last] = m;
 744                 hinfo->q_last++;
 745                 if (hinfo->q_last >= NG_CAR_QUEUE_SIZE)
 746                         hinfo->q_last = 0;
 747
 748                 /* Use RED tokens. */
 749                 if (len > NG_CAR_QUEUE_MIN_TH)
 750                         hinfo->te += len - NG_CAR_QUEUE_MIN_TH;
 751
 752                 /* If this is a first packet in the queue. */
 753                 if (len == 0) {
 754                         if (hinfo->conf.opt & NG_CAR_COUNT_PACKETS) {
 755                                 hinfo->tc -= 128;
 756                         } else {
 757                                 hinfo->tc -= m->m_pkthdr.len;
 758                         }
 759
 760                         /* Schedule queue processing. */
 761                         ng_car_schedule(hinfo);
 762                 }
 763         }
 764
 765         /* Unlock queue mutex. */
 766         mtx_unlock(&hinfo->q_mtx);
 767 }