sys/netgraph/ng_car.c

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