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