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 } 807