sys/netgraph/ng_car.c

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